#include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include MutexIF* ImtqHandler::TORQUE_LOCK = nullptr; bool ImtqHandler::TORQUEING = false; Countdown ImtqHandler::TORQUE_COUNTDOWN = Countdown(); ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, power::Switch_t pwrSwitcher) : DeviceHandlerBase(objectId, comIF, comCookie), engHkDataset(this), calMtmMeasurementSet(this), rawMtmMeasurementSet(this), dipoleSet(*this), posXselfTestDataset(this), negXselfTestDataset(this), posYselfTestDataset(this), negYselfTestDataset(this), posZselfTestDataset(this), negZselfTestDataset(this), switcher(pwrSwitcher) { if (comCookie == nullptr) { sif::error << "IMTQHandler: Invalid com cookie" << std::endl; } TORQUE_LOCK = MutexFactory::instance()->createMutex(); } ImtqHandler::~ImtqHandler() {} void ImtqHandler::doStartUp() { if (goToNormalMode) { setMode(MODE_NORMAL); } else { setMode(_MODE_TO_ON); } } void ImtqHandler::doShutDown() { setMode(_MODE_POWER_DOWN); } ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { // TODO: Add IMTQ torque handling here, will be set in the software and thus must be part // of normal handling. Keep in mind that torqueing should happen after all HK data was read // because the MGM values might be useless if the IMTQ is torqueing. Also, remove // calibrated MGM polling, we have our own calibration. Thus, there are 3 communication // steps per IMTQ cycle. switch (communicationStep) { case CommunicationStep::GET_ENG_HK_DATA: *id = IMTQ::GET_ENG_HK_DATA; communicationStep = CommunicationStep::START_MTM_MEASUREMENT; break; case CommunicationStep::START_MTM_MEASUREMENT: *id = IMTQ::START_MTM_MEASUREMENT; if (pollingMode == NormalPollingMode::BOTH or pollingMode == NormalPollingMode::UNCALIBRATED) { communicationStep = CommunicationStep::GET_RAW_MTM_MEASUREMENT; } else { communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT; } break; case CommunicationStep::GET_RAW_MTM_MEASUREMENT: if (integrationTimeCd.getRemainingMillis() > 0) { TaskFactory::delayTask(integrationTimeCd.getRemainingMillis()); } *id = IMTQ::GET_RAW_MTM_MEASUREMENT; if (pollingMode == NormalPollingMode::BOTH) { communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT; } else { communicationStep = CommunicationStep::DIPOLE_ACTUATION; } break; case CommunicationStep::GET_CAL_MTM_MEASUREMENT: if (integrationTimeCd.getRemainingMillis() > 0) { TaskFactory::delayTask(integrationTimeCd.getRemainingMillis()); } *id = IMTQ::GET_CAL_MTM_MEASUREMENT; communicationStep = CommunicationStep::DIPOLE_ACTUATION; break; case CommunicationStep::DIPOLE_ACTUATION: { // TODO: Set correct ID if actuation is necessary *id = IMTQ::START_ACTUATION_DIPOLE; communicationStep = CommunicationStep::GET_ENG_HK_DATA; break; } default: sif::debug << "IMTQHandler::buildNormalDeviceCommand: Invalid communication step" << std::endl; break; } return buildCommandFromCommand(*id, nullptr, 0); } ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { return NOTHING_TO_SEND; } ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t* commandData, size_t commandDataLen) { switch (deviceCommand) { case (IMTQ::POS_X_SELF_TEST): { commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; commandBuffer[1] = IMTQ::SELF_TEST_AXIS::X_POSITIVE; rawPacket = commandBuffer; rawPacketLen = 2; return returnvalue::OK; } case (IMTQ::NEG_X_SELF_TEST): { commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; commandBuffer[1] = IMTQ::SELF_TEST_AXIS::X_NEGATIVE; rawPacket = commandBuffer; rawPacketLen = 2; return returnvalue::OK; } case (IMTQ::POS_Y_SELF_TEST): { commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Y_POSITIVE; rawPacket = commandBuffer; rawPacketLen = 2; return returnvalue::OK; } case (IMTQ::NEG_Y_SELF_TEST): { commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Y_NEGATIVE; rawPacket = commandBuffer; rawPacketLen = 2; return returnvalue::OK; } case (IMTQ::POS_Z_SELF_TEST): { commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Z_POSITIVE; rawPacket = commandBuffer; rawPacketLen = 2; return returnvalue::OK; } case (IMTQ::NEG_Z_SELF_TEST): { commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Z_NEGATIVE; rawPacket = commandBuffer; rawPacketLen = 2; return returnvalue::OK; } case (IMTQ::GET_SELF_TEST_RESULT): { commandBuffer[0] = IMTQ::CC::GET_SELF_TEST_RESULT; rawPacket = commandBuffer; rawPacketLen = 1; return returnvalue::OK; } case (IMTQ::START_ACTUATION_DIPOLE): { /* IMTQ expects low byte first */ commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE; if (commandData != nullptr && commandDataLen < 8) { return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; } PoolReadGuard pg(&dipoleSet); ReturnValue_t result; // Commands override anything which was set in the software if (commandBuffer != nullptr) { result = dipoleSet.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::NETWORK); if (result != returnvalue::OK) { return result; } } result = buildDipoleActuationCommand(); if (result != returnvalue::OK) { return result; } MutexGuard mg(TORQUE_LOCK); TORQUEING = true; TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value); return result; } case (IMTQ::GET_ENG_HK_DATA): { commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA; rawPacket = commandBuffer; rawPacketLen = 1; return returnvalue::OK; } case (IMTQ::GET_COMMANDED_DIPOLE): { commandBuffer[0] = IMTQ::CC::GET_COMMANDED_DIPOLE; rawPacket = commandBuffer; rawPacketLen = 1; return returnvalue::OK; } case (IMTQ::START_MTM_MEASUREMENT): { commandBuffer[0] = IMTQ::CC::START_MTM_MEASUREMENT; integrationTimeCd.resetTimer(); rawPacket = commandBuffer; rawPacketLen = 1; return returnvalue::OK; } case (IMTQ::GET_CAL_MTM_MEASUREMENT): { commandBuffer[0] = IMTQ::CC::GET_CAL_MTM_MEASUREMENT; rawPacket = commandBuffer; rawPacketLen = 1; return returnvalue::OK; } case (IMTQ::GET_RAW_MTM_MEASUREMENT): { commandBuffer[0] = IMTQ::CC::GET_RAW_MTM_MEASUREMENT; rawPacket = commandBuffer; rawPacketLen = 1; return returnvalue::OK; } default: return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; } return returnvalue::FAILED; } ReturnValue_t ImtqHandler::buildDipoleActuationCommand() { commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE; uint8_t* serPtr = commandBuffer + 1; size_t serSize = 1; dipoleSet.setValidityBufferGeneration(false); ReturnValue_t result = dipoleSet.serialize(&serPtr, &serSize, sizeof(commandBuffer), SerializeIF::Endianness::LITTLE); dipoleSet.setValidityBufferGeneration(true); if (result != returnvalue::OK) { return result; } rawPacket = commandBuffer; rawPacketLen = 9; return result; } void ImtqHandler::fillCommandAndReplyMap() { this->insertInCommandAndReplyMap(IMTQ::POS_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); this->insertInCommandAndReplyMap(IMTQ::NEG_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); this->insertInCommandAndReplyMap(IMTQ::POS_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); this->insertInCommandAndReplyMap(IMTQ::NEG_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); this->insertInCommandAndReplyMap(IMTQ::POS_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); this->insertInCommandAndReplyMap(IMTQ::NEG_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); this->insertInCommandAndReplyMap(IMTQ::GET_SELF_TEST_RESULT, 1, nullptr, IMTQ::SIZE_SELF_TEST_RESULTS); this->insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); this->insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset, IMTQ::SIZE_ENG_HK_DATA_REPLY); this->insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr, IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY); this->insertInCommandAndReplyMap(IMTQ::START_MTM_MEASUREMENT, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); this->insertInCommandAndReplyMap(IMTQ::GET_CAL_MTM_MEASUREMENT, 1, &calMtmMeasurementSet, IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT); this->insertInCommandAndReplyMap(IMTQ::GET_RAW_MTM_MEASUREMENT, 1, &rawMtmMeasurementSet, IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT); } ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) { ReturnValue_t result = returnvalue::OK; switch (*start) { case (IMTQ::CC::START_ACTUATION_DIPOLE): *foundLen = IMTQ::SIZE_STATUS_REPLY; *foundId = IMTQ::START_ACTUATION_DIPOLE; break; case (IMTQ::CC::START_MTM_MEASUREMENT): *foundLen = IMTQ::SIZE_STATUS_REPLY; *foundId = IMTQ::START_MTM_MEASUREMENT; break; case (IMTQ::CC::GET_ENG_HK_DATA): *foundLen = IMTQ::SIZE_ENG_HK_DATA_REPLY; *foundId = IMTQ::GET_ENG_HK_DATA; break; case (IMTQ::CC::GET_COMMANDED_DIPOLE): *foundLen = IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY; *foundId = IMTQ::GET_COMMANDED_DIPOLE; break; case (IMTQ::CC::GET_CAL_MTM_MEASUREMENT): *foundLen = IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT; *foundId = IMTQ::GET_CAL_MTM_MEASUREMENT; break; case (IMTQ::CC::GET_RAW_MTM_MEASUREMENT): *foundLen = IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT; *foundId = IMTQ::GET_RAW_MTM_MEASUREMENT; break; case (IMTQ::CC::SELF_TEST_CMD): *foundLen = IMTQ::SIZE_STATUS_REPLY; result = getSelfTestCommandId(foundId); break; case (IMTQ::CC::GET_SELF_TEST_RESULT): *foundLen = IMTQ::SIZE_SELF_TEST_RESULTS; *foundId = IMTQ::GET_SELF_TEST_RESULT; break; default: sif::debug << "IMTQHandler::scanForReply: Reply contains invalid command code" << std::endl; result = IGNORE_REPLY_DATA; break; } return result; } ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { ReturnValue_t result = returnvalue::OK; result = parseStatusByte(packet); if (result != returnvalue::OK) { return result; } switch (id) { case (IMTQ::POS_X_SELF_TEST): case (IMTQ::NEG_X_SELF_TEST): case (IMTQ::POS_Y_SELF_TEST): case (IMTQ::NEG_Y_SELF_TEST): case (IMTQ::POS_Z_SELF_TEST): case (IMTQ::NEG_Z_SELF_TEST): case (IMTQ::START_ACTUATION_DIPOLE): case (IMTQ::START_MTM_MEASUREMENT): /* Replies only the status byte which is already handled with parseStatusByte */ break; case (IMTQ::GET_ENG_HK_DATA): fillEngHkDataset(packet); break; case (IMTQ::GET_COMMANDED_DIPOLE): handleGetCommandedDipoleReply(packet); break; case (IMTQ::GET_CAL_MTM_MEASUREMENT): fillCalibratedMtmDataset(packet); break; case (IMTQ::GET_RAW_MTM_MEASUREMENT): fillRawMtmDataset(packet); break; case (IMTQ::GET_SELF_TEST_RESULT): handleSelfTestReply(packet); break; default: { sif::debug << "IMTQHandler::interpretDeviceReply: Unknown device reply id" << std::endl; return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; } } return returnvalue::OK; } void ImtqHandler::setNormalDatapoolEntriesInvalid() {} LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) { if (sid == engHkDataset.getSid()) { return &engHkDataset; } else if (sid == calMtmMeasurementSet.getSid()) { return &calMtmMeasurementSet; } else if (sid == rawMtmMeasurementSet.getSid()) { return &rawMtmMeasurementSet; } else if (sid == posXselfTestDataset.getSid()) { return &posXselfTestDataset; } else if (sid == negXselfTestDataset.getSid()) { return &negXselfTestDataset; } else if (sid == posYselfTestDataset.getSid()) { return &posYselfTestDataset; } else if (sid == negYselfTestDataset.getSid()) { return &negYselfTestDataset; } else if (sid == posZselfTestDataset.getSid()) { return &posZselfTestDataset; } else if (sid == negZselfTestDataset.getSid()) { return &negZselfTestDataset; } else { sif::error << "IMTQHandler::getDataSetHandle: Invalid sid" << std::endl; return nullptr; } } uint32_t ImtqHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; } ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { /** Entries of engineering housekeeping dataset */ localDataPoolMap.emplace(IMTQ::DIGITAL_VOLTAGE_MV, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::ANALOG_VOLTAGE_MV, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::DIGITAL_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::ANALOG_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::COIL_X_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::COIL_Y_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::COIL_Z_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::COIL_X_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::COIL_Y_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::COIL_Z_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry({0})); /** Entries of calibrated MTM measurement dataset */ localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, &mgmCalEntry); localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry({0})); /** Entries of raw MTM measurement dataset */ localDataPoolMap.emplace(IMTQ::MTM_RAW, new PoolEntry(3)); localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry({0})); /** INIT measurements for positive X axis test */ localDataPoolMap.emplace(IMTQ::INIT_POS_X_ERR, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_X_RAW_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_X_RAW_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_X_RAW_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_X_CAL_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_X_CAL_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_X_CAL_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_X_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_Y_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_Z_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_X_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_Y_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_Z_TEMPERATURE, new PoolEntry({0})); /** INIT measurements for negative X axis test */ localDataPoolMap.emplace(IMTQ::INIT_NEG_X_ERR, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_X_RAW_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_X_RAW_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_X_RAW_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_X_CAL_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_X_CAL_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_X_CAL_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_X_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_Y_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_Z_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_X_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_Y_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_Z_TEMPERATURE, new PoolEntry({0})); /** INIT measurements for positive Y axis test */ localDataPoolMap.emplace(IMTQ::INIT_POS_Y_ERR, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Y_RAW_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Y_RAW_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Y_RAW_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Y_CAL_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Y_CAL_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Y_CAL_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_X_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_Y_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_Z_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_X_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_Y_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_Z_TEMPERATURE, new PoolEntry({0})); /** INIT measurements for negative Y axis test */ localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_ERR, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_RAW_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_RAW_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_RAW_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_CAL_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_CAL_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_CAL_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_X_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_Y_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_Z_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_X_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_Y_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_Z_TEMPERATURE, new PoolEntry({0})); /** INIT measurements for positive Z axis test */ localDataPoolMap.emplace(IMTQ::INIT_POS_Z_ERR, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Z_RAW_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Z_RAW_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Z_RAW_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Z_CAL_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Z_CAL_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Z_CAL_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_X_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_Y_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_Z_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_X_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_Y_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_Z_TEMPERATURE, new PoolEntry({0})); /** INIT measurements for negative Z axis test */ localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_ERR, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_RAW_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_RAW_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_RAW_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_CAL_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_CAL_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_CAL_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_X_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_Y_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_Z_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_X_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_Y_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_Z_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_X_ERR, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_X_RAW_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_X_RAW_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_X_RAW_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_X_CAL_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_X_CAL_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_X_CAL_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_X_COIL_X_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_X_COIL_Y_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_X_COIL_Z_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_X_COIL_X_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_X_COIL_Y_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_X_COIL_Z_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_X_ERR, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_X_RAW_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_X_RAW_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_X_RAW_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_X_CAL_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_X_CAL_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_X_CAL_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_X_COIL_X_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_X_COIL_Y_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_X_COIL_Z_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_X_COIL_X_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_X_COIL_Y_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_X_COIL_Z_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Y_ERR, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Y_RAW_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Y_RAW_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Y_RAW_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Y_CAL_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Y_CAL_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Y_CAL_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Y_COIL_X_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Y_COIL_Y_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Y_COIL_Z_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Y_COIL_X_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Y_COIL_Y_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Y_COIL_Z_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Y_ERR, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Y_RAW_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Y_RAW_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Y_RAW_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Y_CAL_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Y_CAL_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Y_CAL_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_X_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_Y_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_Z_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_X_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_Y_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_Z_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Z_ERR, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Z_RAW_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Z_RAW_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Z_RAW_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Z_CAL_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Z_CAL_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Z_CAL_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Z_COIL_X_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Z_COIL_Y_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Z_COIL_Z_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Z_COIL_X_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Z_COIL_Y_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::POS_Z_COIL_Z_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Z_ERR, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Z_RAW_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Z_RAW_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Z_RAW_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Z_CAL_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Z_CAL_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Z_CAL_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_X_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_Y_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_Z_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_X_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_Y_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_Z_TEMPERATURE, new PoolEntry({0})); /** FINA measurements for positive X axis test */ localDataPoolMap.emplace(IMTQ::FINA_POS_X_ERR, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_X_RAW_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_X_RAW_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_X_RAW_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_X_CAL_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_X_CAL_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_X_CAL_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_X_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Y_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Z_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_X_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Y_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Z_TEMPERATURE, new PoolEntry({0})); /** FINA measurements for negative X axis test */ localDataPoolMap.emplace(IMTQ::FINA_NEG_X_ERR, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_X_RAW_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_X_RAW_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_X_RAW_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_X_CAL_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_X_CAL_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_X_CAL_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_X_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_Y_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_Z_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_X_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_Y_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_Z_TEMPERATURE, new PoolEntry({0})); /** FINA measurements for positive Y axis test */ localDataPoolMap.emplace(IMTQ::FINA_POS_Y_ERR, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Y_RAW_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Y_RAW_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Y_RAW_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Y_CAL_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Y_CAL_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Y_CAL_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_X_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_Y_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_Z_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_X_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_Y_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_Z_TEMPERATURE, new PoolEntry({0})); /** FINA measurements for negative Y axis test */ localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_ERR, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_RAW_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_RAW_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_RAW_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_CAL_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_CAL_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_CAL_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_X_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_Y_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_Z_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_X_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_Y_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_Z_TEMPERATURE, new PoolEntry({0})); /** FINA measurements for positive Z axis test */ localDataPoolMap.emplace(IMTQ::FINA_POS_Z_ERR, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Z_RAW_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Z_RAW_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Z_RAW_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Z_CAL_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Z_CAL_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Z_CAL_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_X_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_Y_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_Z_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_X_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_Y_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_Z_TEMPERATURE, new PoolEntry({0})); /** FINA measurements for negative Z axis test */ localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_ERR, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_RAW_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_RAW_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_RAW_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_CAL_MAG_X, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_CAL_MAG_Y, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_CAL_MAG_Z, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_X_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_Y_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_Z_CURRENT, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_X_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_Y_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_Z_TEMPERATURE, new PoolEntry({0})); poolManager.subscribeForDiagPeriodicPacket( subdp::DiagnosticsHkPeriodicParams(engHkDataset.getSid(), false, 10.0)); poolManager.subscribeForDiagPeriodicPacket( subdp::DiagnosticsHkPeriodicParams(calMtmMeasurementSet.getSid(), false, 10.0)); poolManager.subscribeForDiagPeriodicPacket( subdp::DiagnosticsHkPeriodicParams(rawMtmMeasurementSet.getSid(), false, 10.0)); return returnvalue::OK; } ReturnValue_t ImtqHandler::getSelfTestCommandId(DeviceCommandId_t* id) { DeviceCommandId_t commandId = getPendingCommand(); switch (commandId) { case IMTQ::POS_X_SELF_TEST: case IMTQ::NEG_X_SELF_TEST: case IMTQ::POS_Y_SELF_TEST: case IMTQ::NEG_Y_SELF_TEST: case IMTQ::POS_Z_SELF_TEST: case IMTQ::NEG_Z_SELF_TEST: *id = commandId; break; default: sif::error << "IMTQHandler::getSelfTestCommandId: Reply does not match to pending " << "command" << std::endl; return UNEXPECTED_SELF_TEST_REPLY; } return returnvalue::OK; } ReturnValue_t ImtqHandler::parseStatusByte(const uint8_t* packet) { uint8_t cmdErrorField = *(packet + 1) & 0xF; switch (cmdErrorField) { case 0: return returnvalue::OK; case 1: sif::error << "IMTQHandler::parseStatusByte: Command rejected without reason" << std::endl; return REJECTED_WITHOUT_REASON; case 2: sif::error << "IMTQHandler::parseStatusByte: Command has invalid command code" << std::endl; return INVALID_COMMAND_CODE; case 3: sif::error << "IMTQHandler::parseStatusByte: Command has missing parameter" << std::endl; return PARAMETER_MISSING; case 4: sif::error << "IMTQHandler::parseStatusByte: Command has invalid parameter" << std::endl; return PARAMETER_INVALID; case 5: sif::error << "IMTQHandler::parseStatusByte: CC unavailable" << std::endl; return CC_UNAVAILABLE; case 7: sif::error << "IMTQHandler::parseStatusByte: IMQT replied internal processing error" << std::endl; return INTERNAL_PROCESSING_ERROR; default: sif::error << "IMTQHandler::parseStatusByte: CMD Error field contains unknown error code " << cmdErrorField << std::endl; return CMD_ERR_UNKNOWN; } } void ImtqHandler::fillEngHkDataset(const uint8_t* packet) { PoolReadGuard rg(&engHkDataset); uint8_t offset = 2; engHkDataset.digitalVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; engHkDataset.analogVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; engHkDataset.digitalCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; engHkDataset.analogCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; engHkDataset.coilXCurrentmA = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; engHkDataset.coilYCurrentmA = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; engHkDataset.coilZCurrentmA = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; engHkDataset.coilXTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); offset += 2; engHkDataset.coilYTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); offset += 2; engHkDataset.coilZTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); offset += 2; size_t dummy = 2; SerializeAdapter::deSerialize(&engHkDataset.mcuTemperature.value, packet + offset, &dummy, SerializeIF::Endianness::LITTLE); engHkDataset.setValidity(true, true); if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "IMTQ digital voltage: " << engHkDataset.digitalVoltageMv << " mV" << std::endl; sif::info << "IMTQ analog voltage: " << engHkDataset.analogVoltageMv << " mV" << std::endl; sif::info << "IMTQ digital current: " << engHkDataset.digitalCurrentmA << " mA" << std::endl; sif::info << "IMTQ analog current: " << engHkDataset.analogCurrentmA << " mA" << std::endl; sif::info << "IMTQ coil X current: " << engHkDataset.coilXCurrentmA << " mA" << std::endl; sif::info << "IMTQ coil Y current: " << engHkDataset.coilYCurrentmA << " mA" << std::endl; sif::info << "IMTQ coil Z current: " << engHkDataset.coilZCurrentmA << " mA" << std::endl; sif::info << "IMTQ coil X temperature: " << engHkDataset.coilXTemperature << " °C" << std::endl; sif::info << "IMTQ coil Y temperature: " << engHkDataset.coilYTemperature << " °C" << std::endl; sif::info << "IMTQ coil Z temperature: " << engHkDataset.coilZTemperature << " °C" << std::endl; sif::info << "IMTQ coil MCU temperature: " << engHkDataset.mcuTemperature << " °C" << std::endl; #endif } } void ImtqHandler::setToGoToNormal(bool enable) { this->goToNormalMode = enable; } void ImtqHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) { if (wiretappingMode == RAW) { /* Data already sent in doGetRead() */ return; } DeviceReplyMap::iterator iter = deviceReplyMap.find(replyId); if (iter == deviceReplyMap.end()) { sif::debug << "IMTQHandler::handleDeviceTM: Unknown reply id" << std::endl; return; } MessageQueueId_t queueId = iter->second.command->second.sendReplyTo; if (queueId == NO_COMMANDER) { return; } ReturnValue_t result = actionHelper.reportData(queueId, replyId, data, dataSize); if (result != returnvalue::OK) { sif::debug << "IMTQHandler::handleDeviceTM: Failed to report data" << std::endl; return; } } void ImtqHandler::handleGetCommandedDipoleReply(const uint8_t* packet) { uint8_t tmData[6]; /* Switching endianess of received dipole values */ tmData[0] = *(packet + 3); tmData[1] = *(packet + 2); tmData[2] = *(packet + 5); tmData[3] = *(packet + 4); tmData[4] = *(packet + 7); tmData[5] = *(packet + 6); handleDeviceTM(tmData, sizeof(tmData), IMTQ::GET_COMMANDED_DIPOLE); } void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) { PoolReadGuard rg(&calMtmMeasurementSet); calMtmMeasurementSet.setValidity(true, true); int8_t offset = 2; calMtmMeasurementSet.mgmXyz[0] = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; calMtmMeasurementSet.mgmXyz[1] = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; calMtmMeasurementSet.mgmXyz[2] = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; calMtmMeasurementSet.coilActuationStatus = (*(packet + offset + 3) << 24) | (*(packet + offset + 2) << 16) | (*(packet + offset + 1) << 8) | (*(packet + offset)); if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "IMTQ calibrated MTM measurement X: " << calMtmMeasurementSet.mgmXyz[0] << " nT" << std::endl; sif::info << "IMTQ calibrated MTM measurement Y: " << calMtmMeasurementSet.mgmXyz[1] << " nT" << std::endl; sif::info << "IMTQ calibrated MTM measurement Z: " << calMtmMeasurementSet.mgmXyz[2] << " nT" << std::endl; sif::info << "IMTQ coil actuation status during MTM measurement: " << (unsigned int)calMtmMeasurementSet.coilActuationStatus.value << std::endl; #endif } } void ImtqHandler::fillRawMtmDataset(const uint8_t* packet) { PoolReadGuard rg(&rawMtmMeasurementSet); unsigned int offset = 2; size_t deSerLen = 16; const uint8_t* dataStart = packet + offset; int32_t xRaw = 0; int32_t yRaw = 0; int32_t zRaw = 0; uint32_t coilActStatus = 0; auto res = SerializeAdapter::deSerialize(&xRaw, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE); if (res != returnvalue::OK) { return; } res = SerializeAdapter::deSerialize(&yRaw, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE); if (res != returnvalue::OK) { return; } res = SerializeAdapter::deSerialize(&zRaw, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE); if (res != returnvalue::OK) { return; } res = SerializeAdapter::deSerialize(&coilActStatus, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE); if (res != returnvalue::OK) { return; } rawMtmMeasurementSet.mtmRawNt[0] = xRaw * 7.5; rawMtmMeasurementSet.mtmRawNt[1] = yRaw * 7.5; rawMtmMeasurementSet.mtmRawNt[2] = zRaw * 7.5; rawMtmMeasurementSet.coilActuationStatus = static_cast(coilActStatus); rawMtmMeasurementSet.setValidity(true, true); if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "IMTQ raw MTM measurement X: " << rawMtmMeasurementSet.mtmRawNt[0] << " nT" << std::endl; sif::info << "IMTQ raw MTM measurement Y: " << rawMtmMeasurementSet.mtmRawNt[1] << " nT" << std::endl; sif::info << "IMTQ raw MTM measurement Z: " << rawMtmMeasurementSet.mtmRawNt[2] << " nT" << std::endl; sif::info << "IMTQ coil actuation status during MTM measurement: " << (unsigned int)rawMtmMeasurementSet.coilActuationStatus.value << std::endl; #endif } } void ImtqHandler::handleSelfTestReply(const uint8_t* packet) { uint16_t offset = 2; checkErrorByte(*(packet + offset), *(packet + offset + 1)); switch (*(packet + IMTQ::MAIN_STEP_OFFSET)) { case IMTQ::SELF_TEST_STEPS::X_POSITIVE: { handlePositiveXSelfTestReply(packet); break; } case IMTQ::SELF_TEST_STEPS::X_NEGATIVE: { handleNegativeXSelfTestReply(packet); break; } case IMTQ::SELF_TEST_STEPS::Y_POSITIVE: { handlePositiveYSelfTestReply(packet); break; } case IMTQ::SELF_TEST_STEPS::Y_NEGATIVE: { handleNegativeYSelfTestReply(packet); break; } case IMTQ::SELF_TEST_STEPS::Z_POSITIVE: { handlePositiveZSelfTestReply(packet); break; } case IMTQ::SELF_TEST_STEPS::Z_NEGATIVE: { handleNegativeZSelfTestReply(packet); break; } default: break; } } void ImtqHandler::handlePositiveXSelfTestReply(const uint8_t* packet) { PoolReadGuard rg(&posXselfTestDataset); uint16_t offset = 2; /** Init measurements */ posXselfTestDataset.initErr = *(packet + offset); offset += 2; // STEP byte will not be stored posXselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posXselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posXselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posXselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posXselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posXselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posXselfTestDataset.initCoilXCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posXselfTestDataset.initCoilYCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posXselfTestDataset.initCoilZCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posXselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; posXselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; posXselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; /** +X measurements */ checkErrorByte(*(packet + offset), *(packet + offset + 1)); posXselfTestDataset.err = *(packet + offset); offset += 2; // STEP byte will not be stored posXselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posXselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posXselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posXselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posXselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posXselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posXselfTestDataset.coilXCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posXselfTestDataset.coilYCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posXselfTestDataset.coilZCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posXselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; posXselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; posXselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; /** FINA measurements */ checkErrorByte(*(packet + offset), *(packet + offset + 1)); posXselfTestDataset.finaErr = *(packet + offset); offset += 2; // STEP byte will not be stored posXselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posXselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posXselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posXselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posXselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posXselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posXselfTestDataset.finaCoilXCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posXselfTestDataset.finaCoilYCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posXselfTestDataset.finaCoilZCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posXselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; posXselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; posXselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "IMTQ self test (INIT) err: " << static_cast(posXselfTestDataset.initErr.value) << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field X: " << posXselfTestDataset.initRawMagX << " nT" << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << posXselfTestDataset.initRawMagY << " nT" << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << posXselfTestDataset.initRawMagZ << " nT" << std::endl; sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " << posXselfTestDataset.initCalMagX << " nT" << std::endl; sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " << posXselfTestDataset.initCalMagY << " nT" << std::endl; sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " << posXselfTestDataset.initCalMagZ << " nT" << std::endl; sif::info << "IMTQ self test (INIT) coil X current: " << posXselfTestDataset.initCoilXCurrent << " mA" << std::endl; sif::info << "IMTQ self test (INIT) coil Y current: " << posXselfTestDataset.initCoilYCurrent << " mA" << std::endl; sif::info << "IMTQ self test (INIT) coil Z current: " << posXselfTestDataset.initCoilZCurrent << " mA" << std::endl; sif::info << "IMTQ self test (INIT) coil X temperature: " << posXselfTestDataset.initCoilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (INIT) coil Y temperature: " << posXselfTestDataset.initCoilYTemperature << " °C" << std::endl; sif::info << "IMTQ self test (INIT) coil Z temperature: " << posXselfTestDataset.initCoilZTemperature << " °C" << std::endl; sif::info << "IMTQ self test (+X) err: " << static_cast(posXselfTestDataset.err.value) << std::endl; sif::info << "IMTQ self test (+X) raw magnetic field X: " << posXselfTestDataset.rawMagX << " nT" << std::endl; sif::info << "IMTQ self test (+X) raw magnetic field Y: " << posXselfTestDataset.rawMagY << " nT" << std::endl; sif::info << "IMTQ self test (+X) raw magnetic field Z: " << posXselfTestDataset.rawMagZ << " nT" << std::endl; sif::info << "IMTQ self test (+X) calibrated magnetic field X: " << posXselfTestDataset.calMagX << " nT" << std::endl; sif::info << "IMTQ self test (+X) calibrated magnetic field Y: " << posXselfTestDataset.calMagY << " nT" << std::endl; sif::info << "IMTQ self test (+X) calibrated magnetic field Z: " << posXselfTestDataset.calMagZ << " nT" << std::endl; sif::info << "IMTQ self test (+X) coil X current: " << posXselfTestDataset.coilXCurrent << " mA" << std::endl; sif::info << "IMTQ self test (+X) coil Y current: " << posXselfTestDataset.coilYCurrent << " mA" << std::endl; sif::info << "IMTQ self test (+X) coil Z current: " << posXselfTestDataset.coilZCurrent << " mA" << std::endl; sif::info << "IMTQ self test (+X) coil X temperature: " << posXselfTestDataset.coilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (+X) coil Y temperature: " << posXselfTestDataset.coilYTemperature << " °C" << std::endl; sif::info << "IMTQ self test (+X) coil Z temperature: " << posXselfTestDataset.coilZTemperature << " °C" << std::endl; sif::info << "IMTQ self test (FINA) err: " << static_cast(posXselfTestDataset.finaErr.value) << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field X: " << posXselfTestDataset.finaRawMagX << " nT" << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << posXselfTestDataset.finaRawMagY << " nT" << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << posXselfTestDataset.finaRawMagZ << " nT" << std::endl; sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " << posXselfTestDataset.finaCalMagX << " nT" << std::endl; sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " << posXselfTestDataset.finaCalMagY << " nT" << std::endl; sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " << posXselfTestDataset.finaCalMagZ << " nT" << std::endl; sif::info << "IMTQ self test (FINA) coil X current: " << posXselfTestDataset.finaCoilXCurrent << " mA" << std::endl; sif::info << "IMTQ self test (FINA) coil Y current: " << posXselfTestDataset.finaCoilYCurrent << " mA" << std::endl; sif::info << "IMTQ self test (FINA) coil Z current: " << posXselfTestDataset.finaCoilZCurrent << " mA" << std::endl; sif::info << "IMTQ self test (FINA) coil X temperature: " << posXselfTestDataset.finaCoilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (FINA) coil Y temperature: " << posXselfTestDataset.finaCoilYTemperature << " °C" << std::endl; sif::info << "IMTQ self test (FINA) coil Z temperature: " << posXselfTestDataset.finaCoilZTemperature << " °C" << std::endl; #endif } } void ImtqHandler::handleNegativeXSelfTestReply(const uint8_t* packet) { PoolReadGuard rg(&posXselfTestDataset); uint16_t offset = 2; /** Init measurements */ negXselfTestDataset.initErr = *(packet + offset); offset += 2; // STEP byte will not be stored negXselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negXselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negXselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negXselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negXselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negXselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negXselfTestDataset.initCoilXCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negXselfTestDataset.initCoilYCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negXselfTestDataset.initCoilZCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negXselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; negXselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; negXselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; /** +X measurements */ checkErrorByte(*(packet + offset), *(packet + offset + 1)); negXselfTestDataset.err = *(packet + offset); offset += 2; // STEP byte will not be stored negXselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negXselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negXselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negXselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negXselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negXselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negXselfTestDataset.coilXCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negXselfTestDataset.coilYCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negXselfTestDataset.coilZCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negXselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; negXselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; negXselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; /** FINA measurements */ checkErrorByte(*(packet + offset), *(packet + offset + 1)); negXselfTestDataset.finaErr = *(packet + offset); offset += 2; // STEP byte will not be stored negXselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negXselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negXselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negXselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negXselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negXselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negXselfTestDataset.finaCoilXCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negXselfTestDataset.finaCoilYCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negXselfTestDataset.finaCoilZCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negXselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; negXselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; negXselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "IMTQ self test (INIT) err: " << static_cast(negXselfTestDataset.initErr.value) << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field X: " << negXselfTestDataset.initRawMagX << " nT" << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << negXselfTestDataset.initRawMagY << " nT" << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << negXselfTestDataset.initRawMagZ << " nT" << std::endl; sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " << negXselfTestDataset.initCalMagX << " nT" << std::endl; sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " << negXselfTestDataset.initCalMagY << " nT" << std::endl; sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " << negXselfTestDataset.initCalMagZ << " nT" << std::endl; sif::info << "IMTQ self test (INIT) coil X current: " << negXselfTestDataset.initCoilXCurrent << " mA" << std::endl; sif::info << "IMTQ self test (INIT) coil Y current: " << negXselfTestDataset.initCoilYCurrent << " mA" << std::endl; sif::info << "IMTQ self test (INIT) coil Z current: " << negXselfTestDataset.initCoilZCurrent << " mA" << std::endl; sif::info << "IMTQ self test (INIT) coil X temperature: " << negXselfTestDataset.initCoilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (INIT) coil Y temperature: " << negXselfTestDataset.initCoilYTemperature << " °C" << std::endl; sif::info << "IMTQ self test (INIT) coil Z temperature: " << negXselfTestDataset.initCoilZTemperature << " °C" << std::endl; sif::info << "IMTQ self test (-X) err: " << static_cast(negXselfTestDataset.err.value) << std::endl; sif::info << "IMTQ self test (-X) raw magnetic field X: " << negXselfTestDataset.rawMagX << " nT" << std::endl; sif::info << "IMTQ self test (-X) raw magnetic field Y: " << negXselfTestDataset.rawMagY << " nT" << std::endl; sif::info << "IMTQ self test (-X) raw magnetic field Z: " << negXselfTestDataset.rawMagZ << " nT" << std::endl; sif::info << "IMTQ self test (-X) calibrated magnetic field X: " << negXselfTestDataset.calMagX << " nT" << std::endl; sif::info << "IMTQ self test (-X) calibrated magnetic field Y: " << negXselfTestDataset.calMagY << " nT" << std::endl; sif::info << "IMTQ self test (-X) calibrated magnetic field Z: " << negXselfTestDataset.calMagZ << " nT" << std::endl; sif::info << "IMTQ self test (-X) coil X current: " << negXselfTestDataset.coilXCurrent << " mA" << std::endl; sif::info << "IMTQ self test (-X) coil Y current: " << negXselfTestDataset.coilYCurrent << " mA" << std::endl; sif::info << "IMTQ self test (-X) coil Z current: " << negXselfTestDataset.coilZCurrent << " mA" << std::endl; sif::info << "IMTQ self test (-X) coil X temperature: " << negXselfTestDataset.coilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (-X) coil Y temperature: " << negXselfTestDataset.coilYTemperature << " °C" << std::endl; sif::info << "IMTQ self test (-X) coil Z temperature: " << negXselfTestDataset.coilZTemperature << " °C" << std::endl; sif::info << "IMTQ self test (FINA) err: " << static_cast(negXselfTestDataset.finaErr.value) << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field X: " << negXselfTestDataset.finaRawMagX << " nT" << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << negXselfTestDataset.finaRawMagY << " nT" << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << negXselfTestDataset.finaRawMagZ << " nT" << std::endl; sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " << negXselfTestDataset.finaCalMagX << " nT" << std::endl; sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " << negXselfTestDataset.finaCalMagY << " nT" << std::endl; sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " << negXselfTestDataset.finaCalMagZ << " nT" << std::endl; sif::info << "IMTQ self test (FINA) coil X current: " << negXselfTestDataset.finaCoilXCurrent << " mA" << std::endl; sif::info << "IMTQ self test (FINA) coil Y current: " << negXselfTestDataset.finaCoilYCurrent << " mA" << std::endl; sif::info << "IMTQ self test (FINA) coil Z current: " << negXselfTestDataset.finaCoilZCurrent << " mA" << std::endl; sif::info << "IMTQ self test (FINA) coil X temperature: " << negXselfTestDataset.finaCoilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (FINA) coil Y temperature: " << negXselfTestDataset.finaCoilYTemperature << " °C" << std::endl; sif::info << "IMTQ self test (FINA) coil Z temperature: " << negXselfTestDataset.finaCoilZTemperature << " °C" << std::endl; #endif } } void ImtqHandler::handlePositiveYSelfTestReply(const uint8_t* packet) { PoolReadGuard rg(&posXselfTestDataset); uint16_t offset = 2; /** Init measurements */ posYselfTestDataset.initErr = *(packet + offset); offset += 2; // STEP byte will not be stored posYselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posYselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posYselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posYselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posYselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posYselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posYselfTestDataset.initCoilXCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posYselfTestDataset.initCoilYCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posYselfTestDataset.initCoilZCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posYselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; posYselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; posYselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; /** +X measurements */ checkErrorByte(*(packet + offset), *(packet + offset + 1)); posYselfTestDataset.err = *(packet + offset); offset += 2; // STEP byte will not be stored posYselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posYselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posYselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posYselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posYselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posYselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posYselfTestDataset.coilXCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posYselfTestDataset.coilYCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posYselfTestDataset.coilZCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posYselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; posYselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; posYselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; /** FINA measurements */ checkErrorByte(*(packet + offset), *(packet + offset + 1)); posYselfTestDataset.finaErr = *(packet + offset); offset += 2; // STEP byte will not be stored posYselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posYselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posYselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posYselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posYselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posYselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posYselfTestDataset.finaCoilXCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posYselfTestDataset.finaCoilYCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posYselfTestDataset.finaCoilZCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posYselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; posYselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; posYselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "IMTQ self test (INIT) err: " << static_cast(posYselfTestDataset.initErr.value) << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field X: " << posYselfTestDataset.initRawMagX << " nT" << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << posYselfTestDataset.initRawMagY << " nT" << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << posYselfTestDataset.initRawMagZ << " nT" << std::endl; sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " << posYselfTestDataset.initCalMagX << " nT" << std::endl; sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " << posYselfTestDataset.initCalMagY << " nT" << std::endl; sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " << posYselfTestDataset.initCalMagZ << " nT" << std::endl; sif::info << "IMTQ self test (INIT) coil X current: " << posYselfTestDataset.initCoilXCurrent << " mA" << std::endl; sif::info << "IMTQ self test (INIT) coil Y current: " << posYselfTestDataset.initCoilYCurrent << " mA" << std::endl; sif::info << "IMTQ self test (INIT) coil Z current: " << posYselfTestDataset.initCoilZCurrent << " mA" << std::endl; sif::info << "IMTQ self test (INIT) coil X temperature: " << posYselfTestDataset.initCoilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (INIT) coil Y temperature: " << posYselfTestDataset.initCoilYTemperature << " °C" << std::endl; sif::info << "IMTQ self test (INIT) coil Z temperature: " << posYselfTestDataset.initCoilZTemperature << " °C" << std::endl; sif::info << "IMTQ self test (+Y) err: " << static_cast(posYselfTestDataset.err.value) << std::endl; sif::info << "IMTQ self test (+Y) raw magnetic field X: " << posYselfTestDataset.rawMagX << " nT" << std::endl; sif::info << "IMTQ self test (+Y) raw magnetic field Y: " << posYselfTestDataset.rawMagY << " nT" << std::endl; sif::info << "IMTQ self test (+Y) raw magnetic field Z: " << posYselfTestDataset.rawMagZ << " nT" << std::endl; sif::info << "IMTQ self test (+Y) calibrated magnetic field X: " << posYselfTestDataset.calMagX << " nT" << std::endl; sif::info << "IMTQ self test (+Y) calibrated magnetic field Y: " << posYselfTestDataset.calMagY << " nT" << std::endl; sif::info << "IMTQ self test (+Y) calibrated magnetic field Z: " << posYselfTestDataset.calMagZ << " nT" << std::endl; sif::info << "IMTQ self test (+Y) coil X current: " << posYselfTestDataset.coilXCurrent << " mA" << std::endl; sif::info << "IMTQ self test (+Y) coil Y current: " << posYselfTestDataset.coilYCurrent << " mA" << std::endl; sif::info << "IMTQ self test (+Y) coil Z current: " << posYselfTestDataset.coilZCurrent << " mA" << std::endl; sif::info << "IMTQ self test (+Y) coil X temperature: " << posYselfTestDataset.coilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (+Y) coil Y temperature: " << posYselfTestDataset.coilYTemperature << " °C" << std::endl; sif::info << "IMTQ self test (+Y) coil Z temperature: " << posYselfTestDataset.coilZTemperature << " °C" << std::endl; sif::info << "IMTQ self test (FINA) err: " << static_cast(posYselfTestDataset.finaErr.value) << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field X: " << posYselfTestDataset.finaRawMagX << " nT" << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << posYselfTestDataset.finaRawMagY << " nT" << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << posYselfTestDataset.finaRawMagZ << " nT" << std::endl; sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " << posYselfTestDataset.finaCalMagX << " nT" << std::endl; sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " << posYselfTestDataset.finaCalMagY << " nT" << std::endl; sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " << posYselfTestDataset.finaCalMagZ << " nT" << std::endl; sif::info << "IMTQ self test (FINA) coil X current: " << posYselfTestDataset.finaCoilXCurrent << " mA" << std::endl; sif::info << "IMTQ self test (FINA) coil Y current: " << posYselfTestDataset.finaCoilYCurrent << " mA" << std::endl; sif::info << "IMTQ self test (FINA) coil Z current: " << posYselfTestDataset.finaCoilZCurrent << " mA" << std::endl; sif::info << "IMTQ self test (FINA) coil X temperature: " << posYselfTestDataset.finaCoilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (FINA) coil Y temperature: " << posYselfTestDataset.finaCoilYTemperature << " °C" << std::endl; sif::info << "IMTQ self test (FINA) coil Z temperature: " << posYselfTestDataset.finaCoilZTemperature << " °C" << std::endl; #endif } } void ImtqHandler::handleNegativeYSelfTestReply(const uint8_t* packet) { PoolReadGuard rg(&posXselfTestDataset); uint16_t offset = 2; /** Init measurements */ posZselfTestDataset.initErr = *(packet + offset); offset += 2; // STEP byte will not be stored negYselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negYselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negYselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negYselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negYselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negYselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negYselfTestDataset.initCoilXCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negYselfTestDataset.initCoilYCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negYselfTestDataset.initCoilZCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negYselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; negYselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; negYselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; /** +X measurements */ checkErrorByte(*(packet + offset), *(packet + offset + 1)); negYselfTestDataset.err = *(packet + offset); offset += 2; // STEP byte will not be stored negYselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negYselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negYselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negYselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negYselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negYselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negYselfTestDataset.coilXCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negYselfTestDataset.coilYCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negYselfTestDataset.coilZCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negYselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; negYselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; negYselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; /** FINA measurements */ checkErrorByte(*(packet + offset), *(packet + offset + 1)); negYselfTestDataset.finaErr = *(packet + offset); offset += 2; // STEP byte will not be stored negYselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negYselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negYselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negYselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negYselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negYselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negYselfTestDataset.finaCoilXCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negYselfTestDataset.finaCoilYCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negYselfTestDataset.finaCoilZCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negYselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; negYselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; negYselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "IMTQ self test (INIT) err: " << static_cast(negYselfTestDataset.initErr.value) << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field X: " << negYselfTestDataset.initRawMagX << " nT" << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << negYselfTestDataset.initRawMagY << " nT" << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << negYselfTestDataset.initRawMagZ << " nT" << std::endl; sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " << negYselfTestDataset.initCalMagX << " nT" << std::endl; sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " << negYselfTestDataset.initCalMagY << " nT" << std::endl; sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " << negYselfTestDataset.initCalMagZ << " nT" << std::endl; sif::info << "IMTQ self test (INIT) coil X current: " << negYselfTestDataset.initCoilXCurrent << " mA" << std::endl; sif::info << "IMTQ self test (INIT) coil Y current: " << negYselfTestDataset.initCoilYCurrent << " mA" << std::endl; sif::info << "IMTQ self test (INIT) coil Z current: " << negYselfTestDataset.initCoilZCurrent << " mA" << std::endl; sif::info << "IMTQ self test (INIT) coil X temperature: " << negYselfTestDataset.initCoilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (INIT) coil Y temperature: " << negYselfTestDataset.initCoilYTemperature << " °C" << std::endl; sif::info << "IMTQ self test (INIT) coil Z temperature: " << negYselfTestDataset.initCoilZTemperature << " °C" << std::endl; sif::info << "IMTQ self test (-Y) err: " << static_cast(negYselfTestDataset.err.value) << std::endl; sif::info << "IMTQ self test (-Y) raw magnetic field X: " << negYselfTestDataset.rawMagX << " nT" << std::endl; sif::info << "IMTQ self test (-Y) raw magnetic field Y: " << negYselfTestDataset.rawMagY << " nT" << std::endl; sif::info << "IMTQ self test (-Y) raw magnetic field Z: " << negYselfTestDataset.rawMagZ << " nT" << std::endl; sif::info << "IMTQ self test (-Y) calibrated magnetic field X: " << negYselfTestDataset.calMagX << " nT" << std::endl; sif::info << "IMTQ self test (-Y) calibrated magnetic field Y: " << negYselfTestDataset.calMagY << " nT" << std::endl; sif::info << "IMTQ self test (-Y) calibrated magnetic field Z: " << negYselfTestDataset.calMagZ << " nT" << std::endl; sif::info << "IMTQ self test (-Y) coil X current: " << negYselfTestDataset.coilXCurrent << " mA" << std::endl; sif::info << "IMTQ self test (-Y) coil Y current: " << negYselfTestDataset.coilYCurrent << " mA" << std::endl; sif::info << "IMTQ self test (-Y) coil Z current: " << negYselfTestDataset.coilZCurrent << " mA" << std::endl; sif::info << "IMTQ self test (-Y) coil X temperature: " << negYselfTestDataset.coilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (-Y) coil Y temperature: " << negYselfTestDataset.coilYTemperature << " °C" << std::endl; sif::info << "IMTQ self test (-Y) coil Z temperature: " << negYselfTestDataset.coilZTemperature << " °C" << std::endl; sif::info << "IMTQ self test (FINA) err: " << static_cast(negYselfTestDataset.finaErr.value) << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field X: " << negYselfTestDataset.finaRawMagX << " nT" << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << negYselfTestDataset.finaRawMagY << " nT" << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << negYselfTestDataset.finaRawMagZ << " nT" << std::endl; sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " << negYselfTestDataset.finaCalMagX << " nT" << std::endl; sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " << negYselfTestDataset.finaCalMagY << " nT" << std::endl; sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " << negYselfTestDataset.finaCalMagZ << " nT" << std::endl; sif::info << "IMTQ self test (FINA) coil X current: " << negYselfTestDataset.finaCoilXCurrent << " mA" << std::endl; sif::info << "IMTQ self test (FINA) coil Y current: " << negYselfTestDataset.finaCoilYCurrent << " mA" << std::endl; sif::info << "IMTQ self test (FINA) coil Z current: " << negYselfTestDataset.finaCoilZCurrent << " mA" << std::endl; sif::info << "IMTQ self test (FINA) coil X temperature: " << negYselfTestDataset.finaCoilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (FINA) coil Y temperature: " << negYselfTestDataset.finaCoilYTemperature << " °C" << std::endl; sif::info << "IMTQ self test (FINA) coil Z temperature: " << negYselfTestDataset.finaCoilZTemperature << " °C" << std::endl; #endif } } void ImtqHandler::handlePositiveZSelfTestReply(const uint8_t* packet) { PoolReadGuard rg(&posXselfTestDataset); uint16_t offset = 2; /** Init measurements */ posZselfTestDataset.initErr = *(packet + offset); offset += 2; // STEP byte will not be stored posZselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posZselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posZselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posZselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posZselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posZselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posZselfTestDataset.initCoilXCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posZselfTestDataset.initCoilYCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posZselfTestDataset.initCoilZCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posZselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; posZselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; posZselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; /** +X measurements */ checkErrorByte(*(packet + offset), *(packet + offset + 1)); posZselfTestDataset.err = *(packet + offset); offset += 2; // STEP byte will not be stored posZselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posZselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posZselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posZselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posZselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posZselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posZselfTestDataset.coilXCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posZselfTestDataset.coilYCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posZselfTestDataset.coilZCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posZselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; posZselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; posZselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; /** FINA measurements */ checkErrorByte(*(packet + offset), *(packet + offset + 1)); posZselfTestDataset.finaErr = *(packet + offset); offset += 2; // STEP byte will not be stored posZselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posZselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posZselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; posZselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posZselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posZselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posZselfTestDataset.finaCoilXCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posZselfTestDataset.finaCoilYCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posZselfTestDataset.finaCoilZCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; posZselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; posZselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; posZselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "IMTQ self test (INIT) err: " << static_cast(posZselfTestDataset.initErr.value) << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field X: " << posZselfTestDataset.initRawMagX << " nT" << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << posZselfTestDataset.initRawMagY << " nT" << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << posZselfTestDataset.initRawMagZ << " nT" << std::endl; sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " << posZselfTestDataset.initCalMagX << " nT" << std::endl; sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " << posZselfTestDataset.initCalMagY << " nT" << std::endl; sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " << posZselfTestDataset.initCalMagZ << " nT" << std::endl; sif::info << "IMTQ self test (INIT) coil X current: " << posZselfTestDataset.initCoilXCurrent << " mA" << std::endl; sif::info << "IMTQ self test (INIT) coil Y current: " << posZselfTestDataset.initCoilYCurrent << " mA" << std::endl; sif::info << "IMTQ self test (INIT) coil Z current: " << posZselfTestDataset.initCoilZCurrent << " mA" << std::endl; sif::info << "IMTQ self test (INIT) coil X temperature: " << posZselfTestDataset.initCoilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (INIT) coil Y temperature: " << posZselfTestDataset.initCoilYTemperature << " °C" << std::endl; sif::info << "IMTQ self test (INIT) coil Z temperature: " << posZselfTestDataset.initCoilZTemperature << " °C" << std::endl; sif::info << "IMTQ self test (+Z) err: " << static_cast(posZselfTestDataset.err.value) << std::endl; sif::info << "IMTQ self test (+Z) raw magnetic field X: " << posZselfTestDataset.rawMagX << " nT" << std::endl; sif::info << "IMTQ self test (+Z) raw magnetic field Y: " << posZselfTestDataset.rawMagY << " nT" << std::endl; sif::info << "IMTQ self test (+Z) raw magnetic field Z: " << posZselfTestDataset.rawMagZ << " nT" << std::endl; sif::info << "IMTQ self test (+Z) calibrated magnetic field X: " << posZselfTestDataset.calMagX << " nT" << std::endl; sif::info << "IMTQ self test (+Z) calibrated magnetic field Y: " << posZselfTestDataset.calMagY << " nT" << std::endl; sif::info << "IMTQ self test (+Z) calibrated magnetic field Z: " << posZselfTestDataset.calMagZ << " nT" << std::endl; sif::info << "IMTQ self test (+Z) coil X current: " << posZselfTestDataset.coilXCurrent << " mA" << std::endl; sif::info << "IMTQ self test (+Z) coil Y current: " << posZselfTestDataset.coilYCurrent << " mA" << std::endl; sif::info << "IMTQ self test (+Z) coil Z current: " << posZselfTestDataset.coilZCurrent << " mA" << std::endl; sif::info << "IMTQ self test (+Z) coil X temperature: " << posZselfTestDataset.coilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (+Z) coil Y temperature: " << posZselfTestDataset.coilYTemperature << " °C" << std::endl; sif::info << "IMTQ self test (+Z) coil Z temperature: " << negYselfTestDataset.coilZTemperature << " °C" << std::endl; sif::info << "IMTQ self test (FINA) err: " << static_cast(posZselfTestDataset.finaErr.value) << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field X: " << posZselfTestDataset.finaRawMagX << " nT" << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << posZselfTestDataset.finaRawMagY << " nT" << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << posZselfTestDataset.finaRawMagZ << " nT" << std::endl; sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " << posZselfTestDataset.finaCalMagX << " nT" << std::endl; sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " << posZselfTestDataset.finaCalMagY << " nT" << std::endl; sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " << posZselfTestDataset.finaCalMagZ << " nT" << std::endl; sif::info << "IMTQ self test (FINA) coil X current: " << posZselfTestDataset.finaCoilXCurrent << " mA" << std::endl; sif::info << "IMTQ self test (FINA) coil Y current: " << posZselfTestDataset.finaCoilYCurrent << " mA" << std::endl; sif::info << "IMTQ self test (FINA) coil Z current: " << posZselfTestDataset.finaCoilZCurrent << " mA" << std::endl; sif::info << "IMTQ self test (FINA) coil X temperature: " << posZselfTestDataset.finaCoilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (FINA) coil Y temperature: " << posZselfTestDataset.finaCoilYTemperature << " °C" << std::endl; sif::info << "IMTQ self test (FINA) coil Z temperature: " << posZselfTestDataset.finaCoilZTemperature << " °C" << std::endl; #endif } } void ImtqHandler::handleNegativeZSelfTestReply(const uint8_t* packet) { PoolReadGuard rg(&posXselfTestDataset); uint16_t offset = 2; /** Init measurements */ negZselfTestDataset.initErr = *(packet + offset); offset += 2; // STEP byte will not be stored negZselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negZselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negZselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negZselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negZselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negZselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negZselfTestDataset.initCoilXCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negZselfTestDataset.initCoilYCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negZselfTestDataset.initCoilZCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negZselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; negZselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; negZselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; /** +X measurements */ checkErrorByte(*(packet + offset), *(packet + offset + 1)); negZselfTestDataset.err = *(packet + offset); offset += 2; // STEP byte will not be stored negZselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negZselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negZselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negZselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negZselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negZselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negZselfTestDataset.coilXCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negZselfTestDataset.coilYCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negZselfTestDataset.coilZCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negZselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; negZselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; negZselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; /** FINA measurements */ checkErrorByte(*(packet + offset), *(packet + offset + 1)); negZselfTestDataset.finaErr = *(packet + offset); offset += 2; // STEP byte will not be stored negZselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negZselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negZselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; offset += 4; negZselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negZselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negZselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; negZselfTestDataset.finaCoilXCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negZselfTestDataset.finaCoilYCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negZselfTestDataset.finaCoilZCurrent = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; negZselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; negZselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; negZselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "IMTQ self test (INIT) err: " << static_cast(negZselfTestDataset.initErr.value) << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field X: " << negZselfTestDataset.initRawMagX << " nT" << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << negZselfTestDataset.initRawMagY << " nT" << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << negZselfTestDataset.initRawMagZ << " nT" << std::endl; sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " << negZselfTestDataset.initCalMagX << " nT" << std::endl; sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " << negZselfTestDataset.initCalMagY << " nT" << std::endl; sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " << negZselfTestDataset.initCalMagZ << " nT" << std::endl; sif::info << "IMTQ self test (INIT) coil X current: " << negZselfTestDataset.initCoilXCurrent << " mA" << std::endl; sif::info << "IMTQ self test (INIT) coil Y current: " << negZselfTestDataset.initCoilYCurrent << " mA" << std::endl; sif::info << "IMTQ self test (INIT) coil Z current: " << negZselfTestDataset.initCoilZCurrent << " mA" << std::endl; sif::info << "IMTQ self test (INIT) coil X temperature: " << negZselfTestDataset.initCoilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (INIT) coil Y temperature: " << negZselfTestDataset.initCoilYTemperature << " °C" << std::endl; sif::info << "IMTQ self test (INIT) coil Z temperature: " << negZselfTestDataset.initCoilZTemperature << " °C" << std::endl; sif::info << "IMTQ self test (-Z) err: " << static_cast(negZselfTestDataset.err.value) << std::endl; sif::info << "IMTQ self test (-Z) raw magnetic field X: " << negZselfTestDataset.rawMagX << " nT" << std::endl; sif::info << "IMTQ self test (-Z) raw magnetic field Y: " << negZselfTestDataset.rawMagY << " nT" << std::endl; sif::info << "IMTQ self test (-Z) raw magnetic field Z: " << negZselfTestDataset.rawMagZ << " nT" << std::endl; sif::info << "IMTQ self test (-Z) calibrated magnetic field X: " << negZselfTestDataset.calMagX << " nT" << std::endl; sif::info << "IMTQ self test (-Z) calibrated magnetic field Y: " << negZselfTestDataset.calMagY << " nT" << std::endl; sif::info << "IMTQ self test (-Z) calibrated magnetic field Z: " << negZselfTestDataset.calMagZ << " nT" << std::endl; sif::info << "IMTQ self test (-Z) coil X current: " << negZselfTestDataset.coilXCurrent << " mA" << std::endl; sif::info << "IMTQ self test (-Z) coil Y current: " << negZselfTestDataset.coilYCurrent << " mA" << std::endl; sif::info << "IMTQ self test (-Z) coil Z current: " << negZselfTestDataset.coilZCurrent << " mA" << std::endl; sif::info << "IMTQ self test (-Z) coil X temperature: " << negZselfTestDataset.coilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (-Z) coil Y temperature: " << negZselfTestDataset.coilYTemperature << " °C" << std::endl; sif::info << "IMTQ self test (-Z) coil Z temperature: " << negYselfTestDataset.coilZTemperature << " °C" << std::endl; sif::info << "IMTQ self test (FINA) err: " << static_cast(negZselfTestDataset.finaErr.value) << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field X: " << negZselfTestDataset.finaRawMagX << " nT" << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << negZselfTestDataset.finaRawMagY << " nT" << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << negZselfTestDataset.finaRawMagZ << " nT" << std::endl; sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " << negZselfTestDataset.finaCalMagX << " nT" << std::endl; sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " << negZselfTestDataset.finaCalMagY << " nT" << std::endl; sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " << negZselfTestDataset.finaCalMagZ << " nT" << std::endl; sif::info << "IMTQ self test (FINA) coil X current: " << negZselfTestDataset.finaCoilXCurrent << " mA" << std::endl; sif::info << "IMTQ self test (FINA) coil Y current: " << negZselfTestDataset.finaCoilYCurrent << " mA" << std::endl; sif::info << "IMTQ self test (FINA) coil Z current: " << negZselfTestDataset.finaCoilZCurrent << " mA" << std::endl; sif::info << "IMTQ self test (FINA) coil X temperature: " << negZselfTestDataset.finaCoilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (FINA) coil Y temperature: " << negZselfTestDataset.finaCoilYTemperature << " °C" << std::endl; sif::info << "IMTQ self test (FINA) coil Z temperature: " << negZselfTestDataset.finaCoilZTemperature << " °C" << std::endl; #endif } } void ImtqHandler::setDebugMode(bool enable) { this->debugMode = enable; } void ImtqHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) { std::string stepString(""); if (step < 8) { stepString = makeStepString(step); } else { /** This should normally never happen */ sif::debug << "IMTQHandler::checkErrorByte: Invalid step" << std::endl; return; } if (errorByte == 0) { return; } if (errorByte & IMTQ::I2C_FAILURE_MASK) { triggerEvent(SELF_TEST_I2C_FAILURE, step); sif::error << "IMTQHandler::checkErrorByte: Self test I2C failure for step " << stepString << std::endl; } if (errorByte & IMTQ::SPI_FAILURE_MASK) { triggerEvent(SELF_TEST_SPI_FAILURE, step); sif::error << "IMTQHandler::checkErrorByte: Self test SPI failure for step " << stepString << std::endl; } if (errorByte & IMTQ::ADC_FAILURE_MASK) { triggerEvent(SELF_TEST_ADC_FAILURE, step); sif::error << "IMTQHandler::checkErrorByte: Self test ADC failure for step " << stepString << std::endl; } if (errorByte & IMTQ::PWM_FAILURE_MASK) { triggerEvent(SELF_TEST_PWM_FAILURE, step); sif::error << "IMTQHandler::checkErrorByte: Self test PWM failure for step " << stepString << std::endl; } if (errorByte & IMTQ::TC_FAILURE_MASK) { triggerEvent(SELF_TEST_TC_FAILURE, step); sif::error << "IMTQHandler::checkErrorByte: Self test TC failure (system failure) for step " << stepString << std::endl; } if (errorByte & IMTQ::MTM_RANGE_FAILURE_MASK) { triggerEvent(SELF_TEST_TC_FAILURE, step); sif::error << "IMTQHandler::checkErrorByte: Self test MTM range failure for step " << stepString << std::endl; } if (errorByte & IMTQ::COIL_CURRENT_FAILURE_MASK) { triggerEvent(SELF_TEST_COIL_CURRENT_FAILURE, step); sif::error << "IMTQHandler::checkErrorByte: Self test coil current outside of expected " "range for step " << stepString << std::endl; } if (errorByte & IMTQ::INVALID_ERROR_BYTE) { triggerEvent(INVALID_ERROR_BYTE, step); sif::error << "IMTQHandler::checkErrorByte: Self test result of step " << stepString << " has invalid error byte" << std::endl; } } std::string ImtqHandler::makeStepString(const uint8_t step) { std::string stepString(""); switch (step) { case IMTQ::SELF_TEST_STEPS::INIT: stepString = std::string("INIT"); break; case IMTQ::SELF_TEST_STEPS::X_POSITIVE: stepString = std::string("+X"); break; case IMTQ::SELF_TEST_STEPS::X_NEGATIVE: stepString = std::string("-X"); break; case IMTQ::SELF_TEST_STEPS::Y_POSITIVE: stepString = std::string("+Y"); break; case IMTQ::SELF_TEST_STEPS::Y_NEGATIVE: stepString = std::string("-Y"); break; case IMTQ::SELF_TEST_STEPS::Z_POSITIVE: stepString = std::string("+Z"); break; case IMTQ::SELF_TEST_STEPS::Z_NEGATIVE: stepString = std::string("-Z"); break; case IMTQ::SELF_TEST_STEPS::FINA: stepString = std::string("FINA"); break; default: sif::error << "IMTQHandler::checkErrorByte: Received packet with invalid step information" << std::endl; break; } return stepString; } ReturnValue_t ImtqHandler::getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) { if (switcher != power::NO_SWITCH) { *numberOfSwitches = 1; *switches = &switcher; return returnvalue::OK; } return DeviceHandlerBase::NO_SWITCH; } bool ImtqHandler::mgtIsTorqueing(dur_millis_t* remainingTorqueDuration) { MutexGuard mg(TORQUE_LOCK); if (TORQUEING and remainingTorqueDuration != nullptr) { *remainingTorqueDuration = TORQUE_COUNTDOWN.getRemainingMillis() + TORQUE_BUFFER_TIME_MS; } return TORQUEING; }