diff --git a/common/config/commonSubsystemIds.h b/common/config/commonSubsystemIds.h index e06de2a1..0bc3e1dd 100644 --- a/common/config/commonSubsystemIds.h +++ b/common/config/commonSubsystemIds.h @@ -12,6 +12,7 @@ enum: uint8_t { HEATER_HANDLER = 109, SA_DEPL_HANDLER = 110, PLOC_HANDLER = 111, + IMTQ_HANDLER = 112, COMMON_SUBSYSTEM_ID_END }; } diff --git a/linux/fsfwconfig/OBSWConfig.h b/linux/fsfwconfig/OBSWConfig.h index 89bbfe46..1d5c31ff 100644 --- a/linux/fsfwconfig/OBSWConfig.h +++ b/linux/fsfwconfig/OBSWConfig.h @@ -42,6 +42,7 @@ debugging. */ #define DEBUG_RAD_SENSOR 1 #define DEBUG_SUS 1 #define DEBUG_RTD 1 +#define IMTQ_DEBUG 1 // Leave at one as the BSP is linux. Used by the ADIS16507 device handler #define OBSW_ADIS16507_LINUX_COM_IF 1 diff --git a/mission/devices/IMTQHandler.cpp b/mission/devices/IMTQHandler.cpp index 5751dd50..1d9d306f 100644 --- a/mission/devices/IMTQHandler.cpp +++ b/mission/devices/IMTQHandler.cpp @@ -7,7 +7,7 @@ IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) : DeviceHandlerBase(objectId, comIF, comCookie), engHkDataset(this), calMtmMeasurementSet( - this), rawMtmMeasurementSet(this) { + this), rawMtmMeasurementSet(this), selfTestDataset(this) { if (comCookie == NULL) { sif::error << "IMTQHandler: Invalid com cookie" << std::endl; } @@ -18,10 +18,13 @@ IMTQHandler::~IMTQHandler() { void IMTQHandler::doStartUp(){ - if(mode == _MODE_START_UP){ - //TODO: Set to MODE_ON again - setMode(MODE_NORMAL); - communicationStep = CommunicationStep::SELF_TEST; + if(selfTestSuccess == true){ +#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1 + setMode(MODE_NORMAL); +#else + setMode(_MODE_TO_ON); +#endif + selfTestSuccess = false; } } @@ -32,12 +35,6 @@ void IMTQHandler::doShutDown(){ ReturnValue_t IMTQHandler::buildNormalDeviceCommand( DeviceCommandId_t * id) { switch (communicationStep) { - case CommunicationStep::SELF_TEST: -// *id = IMTQ::SELF_TEST; -//TODO: Implementing self test command. On-hold because of issue with humidity in clean -// room - communicationStep = CommunicationStep::GET_ENG_HK_DATA; - break; case CommunicationStep::GET_ENG_HK_DATA: *id = IMTQ::GET_ENG_HK_DATA; communicationStep = CommunicationStep::START_MTM_MEASUREMENT; @@ -64,6 +61,19 @@ ReturnValue_t IMTQHandler::buildNormalDeviceCommand( ReturnValue_t IMTQHandler::buildTransitionDeviceCommand( DeviceCommandId_t * id){ + if (mode == _MODE_START_UP) { + switch (startupStep) { + case StartupStep::COMMAND_SELF_TEST: + *id = IMTQ::SELF_TEST_CMD; + break; + case StartupStep::GET_SELF_TEST_RESULT: + *id = IMTQ::GET_SELF_TEST_RESULT; + break; + default: + sif::debug << "IMTQHandler::buildTransitionDeviceCommand: Unknown startup step" + << std::endl; + } + } return HasReturnvaluesIF::RETURN_OK; } @@ -71,6 +81,19 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand( DeviceCommandId_t deviceCommand, const uint8_t * commandData, size_t commandDataLen) { switch(deviceCommand) { + case(IMTQ::SELF_TEST_CMD): { + commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; + commandBuffer[1] = IMTQ::SELF_TEST_AXIS::ALL; + rawPacket = commandBuffer; + rawPacketLen = 2; + return RETURN_OK; + } + case(IMTQ::GET_SELF_TEST_RESULT): { + commandBuffer[0] = IMTQ::CC::GET_SELF_TEST_RESULT; + rawPacket = commandBuffer; + rawPacketLen = 1; + return RETURN_OK; + } case(IMTQ::START_ACTUATION_DIPOLE): { /* IMTQ expects low byte first */ commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE; @@ -123,6 +146,8 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand( } void IMTQHandler::fillCommandAndReplyMap() { + this->insertInCommandAndReplyMap(IMTQ::SELF_TEST_CMD, 1, nullptr, + IMTQ::SIZE_STATUS_REPLY); this->insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); this->insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset, @@ -188,6 +213,7 @@ ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, } switch (id) { + case (IMTQ::SELF_TEST_CMD): case (IMTQ::START_ACTUATION_DIPOLE): case (IMTQ::START_MTM_MEASUREMENT): /* Replies only the status byte which is already handled with parseStatusByte */ @@ -204,6 +230,9 @@ ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, 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; @@ -218,7 +247,7 @@ void IMTQHandler::setNormalDatapoolEntriesInvalid(){ } uint32_t IMTQHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo){ - return 500; + return 1000; } ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, @@ -249,6 +278,119 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat localDataPoolMap.emplace(IMTQ::MTM_RAW_Z, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry( { 0 })); + /** Entries of dataset for self test results */ + localDataPoolMap.emplace(IMTQ::INIT_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_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 })); + + localDataPoolMap.emplace(IMTQ::FINA_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + return HasReturnvaluesIF::RETURN_OK; } @@ -284,6 +426,7 @@ ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) { } void IMTQHandler::fillEngHkDataset(const uint8_t* packet) { + PoolReadGuard rg(&engHkDataset); uint8_t offset = 2; engHkDataset.digitalVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; @@ -307,7 +450,7 @@ void IMTQHandler::fillEngHkDataset(const uint8_t* packet) { offset += 2; engHkDataset.mcuTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); -#if OBSW_VERBOSE_LEVEL >= 1 && IMQT_DEBUG == 1 +#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 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; @@ -368,6 +511,7 @@ void IMTQHandler::handleGetCommandedDipoleReply(const uint8_t* packet) { } void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) { + PoolReadGuard rg(&calMtmMeasurementSet); int8_t offset = 2; calMtmMeasurementSet.mtmXnT = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); @@ -380,7 +524,7 @@ void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) { offset += 4; calMtmMeasurementSet.coilActuationStatus = (*(packet + offset + 3) << 24) | (*(packet + offset + 2) << 16) | (*(packet + offset + 1) << 8) | (*(packet + offset)); -#if OBSW_VERBOSE_LEVEL >= 1 && IMQT_DEBUG == 1 +#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1 sif::info << "IMTQ calibrated MTM measurement X: " << calMtmMeasurementSet.mtmXnT << " nT" << std::endl; sif::info << "IMTQ calibrated MTM measurement Y: " << calMtmMeasurementSet.mtmYnT << " nT" @@ -393,6 +537,7 @@ void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) { } void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) { + PoolReadGuard rg(&rawMtmMeasurementSet); int8_t offset = 2; rawMtmMeasurementSet.mtmXnT = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; @@ -405,7 +550,7 @@ void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) { offset += 4; rawMtmMeasurementSet.coilActuationStatus = (*(packet + offset + 3) << 24) | (*(packet + offset + 2) << 16) | (*(packet + offset + 1) << 8) | (*(packet + offset)); -#if OBSW_VERBOSE_LEVEL >= 1 && IMQT_DEBUG == 1 +#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1 sif::info << "IMTQ raw MTM measurement X: " << rawMtmMeasurementSet.mtmXnT << " nT" << std::endl; sif::info << "IMTQ raw MTM measurement Y: " << rawMtmMeasurementSet.mtmYnT << " nT" @@ -416,3 +561,587 @@ void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) { << (unsigned int) rawMtmMeasurementSet.coilActuationStatus.value << std::endl; #endif } + +void IMTQHandler::handleSelfTestReply(const uint8_t* packet) { + PoolReadGuard rg(&selfTestDataset); + + /** Init measurements */ + int8_t offset = 2; + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + selfTestDataset.initErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + selfTestDataset.initRawMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.initRawMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.initRawMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.initCoilXCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.initCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.initCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** +X measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + selfTestDataset.posXErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + selfTestDataset.posXRawMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.posXRawMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.posXRawMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.posXCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.posXCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.posXCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.posXCoilXCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.posXCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.posXCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.posXCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.posXCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.posXCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + + /** -X measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + selfTestDataset.negXErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + selfTestDataset.negXRawMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.negXRawMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.negXRawMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.negXCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.negXCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.negXCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.negXCoilXCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.negXCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.negXCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.negXCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.negXCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.negXCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** +Y measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + selfTestDataset.posYErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + selfTestDataset.posYRawMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.posYRawMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.posYRawMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.posYCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.posYCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.posYCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.posYCoilXCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.posYCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.posYCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.posYCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.posYCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.posYCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** -Y measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + selfTestDataset.negYErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + selfTestDataset.negYRawMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.negYRawMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.negYRawMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.negYCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.negYCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.negYCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.negYCoilXCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.negYCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.negYCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.negYCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.negYCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.negYCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** +Z measurements */ + selfTestDataset.posZErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + selfTestDataset.posZRawMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.posZRawMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.posZRawMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.posZCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.posZCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.posZCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.posZCoilXCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.posZCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.posZCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.posZCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.posZCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.posZCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** -Z measurements */ + selfTestDataset.negZErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + selfTestDataset.negZRawMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.negZRawMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.negZRawMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.negZCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.negZCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.negZCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.negZCoilXCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.negZCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.negZCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.negZCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.negZCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.negZCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** FINA measurements */ + selfTestDataset.finaErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + selfTestDataset.finaRawMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.finaRawMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.finaRawMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + selfTestDataset.finaCoilXCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.finaCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.finaCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + selfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + +#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1 + sif::info << "IMTQ self test (INIT) err: " + << static_cast(selfTestDataset.initErr.value) << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field X: " << selfTestDataset.initRawMagX + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << selfTestDataset.initRawMagY + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << selfTestDataset.initRawMagZ + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " << selfTestDataset.initCalMagX + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " << selfTestDataset.initCalMagY + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " << selfTestDataset.initCalMagZ + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (INIT) coil X current: " << selfTestDataset.initCoilXCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y current: " << selfTestDataset.initCoilYCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z current: " << selfTestDataset.initCoilZCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (INIT) coil X temperature: " + << selfTestDataset.initCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y temperature: " + << selfTestDataset.initCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z temperature: " + << selfTestDataset.initCoilZTemperature << " °C" << std::endl; + + sif::info << "IMTQ self test (+X) err: " << + static_cast(selfTestDataset.posXErr.value) << std::endl; + sif::info << "IMTQ self test (+X) raw magnetic field X: " << selfTestDataset.posXRawMagX + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (+X) raw magnetic field Y: " << selfTestDataset.posXRawMagY + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (+X) raw magnetic field Z: " << selfTestDataset.posXRawMagZ + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (+X) calibrated magnetic field X: " << selfTestDataset.posXCalMagX + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (+X) calibrated magnetic field Y: " << selfTestDataset.posXCalMagY + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (+X) calibrated magnetic field Z: " << selfTestDataset.posXCalMagZ + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (+X) coil X current: " << selfTestDataset.posXCoilXCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (+X) coil Y current: " << selfTestDataset.posXCoilYCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (+X) coil Z current: " << selfTestDataset.initCoilZCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (+X) coil X temperature: " + << selfTestDataset.posXCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (+X) coil Y temperature: " + << selfTestDataset.posXCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (+X) coil Z temperature: " + << selfTestDataset.posXCoilZTemperature << " °C" << std::endl; + + sif::info << "IMTQ self test (-X) err: " + << static_cast(selfTestDataset.negXErr.value) << std::endl; + sif::info << "IMTQ self test (-X) raw magnetic field X: " << selfTestDataset.negXRawMagX + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (-X) raw magnetic field Y: " << selfTestDataset.negXRawMagY + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (-X) raw magnetic field Z: " << selfTestDataset.negXRawMagZ + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (-X) calibrated magnetic field X: " << selfTestDataset.negXCalMagX + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (-X) calibrated magnetic field Y: " << selfTestDataset.negXCalMagY + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (-X) calibrated magnetic field Z: " << selfTestDataset.negXCalMagZ + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (-X) coil X current: " << selfTestDataset.negXCoilXCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (-X) coil Y current: " << selfTestDataset.negXCoilYCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (-X) coil Z current: " << selfTestDataset.negXCoilZCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (-X) coil X temperature: " + << selfTestDataset.negXCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (-X) coil Y temperature: " + << selfTestDataset.negXCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (-X) coil Z temperature: " + << selfTestDataset.negXCoilZTemperature << " °C" << std::endl; + + sif::info << "IMTQ self test (+Y) err: " + << static_cast(selfTestDataset.posYErr.value) << std::endl; + sif::info << "IMTQ self test (+Y) raw magnetic field X: " << selfTestDataset.posYRawMagX + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (+Y) raw magnetic field Y: " << selfTestDataset.posYRawMagY + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (+Y) raw magnetic field Z: " << selfTestDataset.posYRawMagZ + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (+Y) calibrated magnetic field X: " << selfTestDataset.posYCalMagX + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (+Y) calibrated magnetic field Y: " << selfTestDataset.posYCalMagY + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (+Y) calibrated magnetic field Z: " << selfTestDataset.posYCalMagZ + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (+Y) coil X current: " << selfTestDataset.posYCoilXCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (+Y) coil Y current: " << selfTestDataset.posYCoilYCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (+Y) coil Z current: " << selfTestDataset.posYCoilZCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (+Y) coil X temperature: " + << selfTestDataset.posYCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (+Y) coil Y temperature: " + << selfTestDataset.posYCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (+Y) coil Z temperature: " + << selfTestDataset.posYCoilZTemperature << " °C" << std::endl; + + sif::info << "IMTQ self test (-Y) err: " + << static_cast(selfTestDataset.negYErr.value) << std::endl; + sif::info << "IMTQ self test (-Y) raw magnetic field X: " << selfTestDataset.negYRawMagX + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (-Y) raw magnetic field Y: " << selfTestDataset.negYRawMagY + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (-Y) raw magnetic field Z: " << selfTestDataset.negYRawMagZ + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (-Y) calibrated magnetic field X: " << selfTestDataset.negYCalMagX + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (-Y) calibrated magnetic field Y: " << selfTestDataset.negYCalMagY + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (-Y) calibrated magnetic field Z: " << selfTestDataset.negYCalMagZ + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (-Y) coil X current: " << selfTestDataset.negYCoilXCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (-Y) coil Y current: " << selfTestDataset.negYCoilYCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (-Y) coil Z current: " << selfTestDataset.negYCoilZCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (-Y) coil X temperature: " + << selfTestDataset.negYCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (-Y) coil Y temperature: " + << selfTestDataset.negYCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (-Y) coil Z temperature: " + << selfTestDataset.negYCoilZTemperature << " °C" << std::endl; + + sif::info << "IMTQ self test (+Z) err: " + << static_cast(selfTestDataset.posZErr.value) << std::endl; + sif::info << "IMTQ self test (+Z) raw magnetic field X: " << selfTestDataset.posZRawMagX + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (+Z) raw magnetic field Y: " << selfTestDataset.posZRawMagY + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (+Z) raw magnetic field Z: " << selfTestDataset.posZRawMagZ + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (+Z) calibrated magnetic field X: " << selfTestDataset.posZCalMagX + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (+Z) calibrated magnetic field Y: " << selfTestDataset.posZCalMagY + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (+Z) calibrated magnetic field Z: " << selfTestDataset.posZCalMagZ + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (+Z) coil X current: " << selfTestDataset.posZCoilXCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (+Z) coil Y current: " << selfTestDataset.posZCoilYCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (+Z) coil Z current: " << selfTestDataset.posZCoilZCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (+Z) coil X temperature: " + << selfTestDataset.posZCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (+Z) coil Y temperature: " + << selfTestDataset.posZCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (+Z) coil Z temperature: " + << selfTestDataset.posZCoilZTemperature << " °C" << std::endl; + + sif::info << "IMTQ self test (-Z) err: " + << static_cast(selfTestDataset.negZErr.value) << std::endl; + sif::info << "IMTQ self test (-Z) raw magnetic field X: " << selfTestDataset.negZRawMagX + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (-Z) raw magnetic field Y: " << selfTestDataset.negZRawMagY + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (-Z) raw magnetic field Z: " << selfTestDataset.negZRawMagZ + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (-Z) calibrated magnetic field X: " << selfTestDataset.negZCalMagX + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (-Z) calibrated magnetic field Y: " << selfTestDataset.negZCalMagY + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (-Z) calibrated magnetic field Z: " << selfTestDataset.negZCalMagZ + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (-Z) coil X current: " << selfTestDataset.negZCoilXCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (-Z) coil Y current: " << selfTestDataset.negZCoilYCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (-Z) coil Z current: " << selfTestDataset.negZCoilZCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (-Z) coil X temperature: " + << selfTestDataset.negZCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (-Z) coil Y temperature: " + << selfTestDataset.negZCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (-Z) coil Z temperature: " + << selfTestDataset.negZCoilZTemperature << " °C" << std::endl; + + sif::info << "IMTQ self test (FINA) err: " + << static_cast(selfTestDataset.finaErr.value) << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field X: " << selfTestDataset.finaRawMagX + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << selfTestDataset.finaRawMagY + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << selfTestDataset.finaRawMagZ + << " 7.5 * 10^-9 T" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " << selfTestDataset.finaCalMagX + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " << selfTestDataset.finaCalMagY + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " << selfTestDataset.finaCalMagZ + << " 10^-9 T" << std::endl; + sif::info << "IMTQ self test (FINA) coil X current: " << selfTestDataset.finaCoilXCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y current: " << selfTestDataset.finaCoilYCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z current: " << selfTestDataset.finaCoilZCurrent + << " 10^-4 A" << std::endl; + sif::info << "IMTQ self test (FINA) coil X temperature: " + << selfTestDataset.finaCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y temperature: " + << selfTestDataset.finaCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z temperature: " + << selfTestDataset.finaCoilZTemperature << " °C" << std::endl; +#endif +} + +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 & 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; + } +} + +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; +} + diff --git a/mission/devices/IMTQHandler.h b/mission/devices/IMTQHandler.h index e162c1c6..5565b261 100644 --- a/mission/devices/IMTQHandler.h +++ b/mission/devices/IMTQHandler.h @@ -50,15 +50,38 @@ private: static const ReturnValue_t REJECTED_WITHOUT_REASON = MAKE_RETURN_CODE(0xA5); static const ReturnValue_t CMD_ERR_UNKNOWN = MAKE_RETURN_CODE(0xA6); + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::IMTQ_HANDLER; + + //! [EXPORT] : [COMMENT] Get self test result returns I2C failure + //! P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA + static const Event SELF_TEST_I2C_FAILURE = MAKE_EVENT(1, severity::LOW); + //! [EXPORT] : [COMMENT] Get self test result returns SPI failure. This concerns the MTM connectivity. + //! P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA + static const Event SELF_TEST_SPI_FAILURE = MAKE_EVENT(2, severity::LOW); + //! [EXPORT] : [COMMENT] Get self test result returns failure in measurement of current and temperature. + //! P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA + static const Event SELF_TEST_ADC_FAILURE = MAKE_EVENT(3, severity::LOW); + //! [EXPORT] : [COMMENT] Get self test result returns PWM failure which concerns the coil actuation. + //! P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA + static const Event SELF_TEST_PWM_FAILURE = MAKE_EVENT(4, severity::LOW); + //! [EXPORT] : [COMMENT] Get self test result returns TC failure (system failure) + //! P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA + static const Event SELF_TEST_TC_FAILURE = MAKE_EVENT(5, severity::LOW); + //! [EXPORT] : [COMMENT] Get self test result returns failure that MTM values were outside of the expected range. + //! P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA + static const Event SELF_TEST_MTM_RANGE_FAILURE = MAKE_EVENT(6, severity::LOW); + //! [EXPORT] : [COMMENT] Get self test result returns failure indicating that the coil current was outside of the expected range + //! P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA + static const Event SELF_TEST_COIL_CURRENT_FAILURE = MAKE_EVENT(7, severity::LOW); IMTQ::EngHkDataset engHkDataset; IMTQ::CalibratedMtmMeasurementSet calMtmMeasurementSet; IMTQ::RawMtmMeasurementSet rawMtmMeasurementSet; + IMTQ::SelfTestDataset selfTestDataset; uint8_t commandBuffer[IMTQ::MAX_COMMAND_SIZE]; enum class CommunicationStep { - SELF_TEST, GET_ENG_HK_DATA, START_MTM_MEASUREMENT, GET_CAL_MTM_MEASUREMENT, @@ -67,6 +90,15 @@ private: CommunicationStep communicationStep = CommunicationStep::GET_ENG_HK_DATA; + enum class StartupStep { + COMMAND_SELF_TEST, + GET_SELF_TEST_RESULT + }; + + StartupStep startupStep = StartupStep::COMMAND_SELF_TEST; + + bool selfTestSuccess = false; + /** * @brief Each reply contains a status byte giving information about a request. This function * parses this byte and returns the associated failure message. @@ -114,6 +146,23 @@ private: * command. */ void fillRawMtmDataset(const uint8_t* packet); + + /** + * @brief This function fills the self test dataset and parses error bytes by means of the + * self test reply data. + * @param packet Pointer to the reply data holding the self test results. + */ + void handleSelfTestReply(const uint8_t* packet); + + /** + * @brief This function checks the error byte of a self test measurement. + * + * @param errorByte The received error byte to check + * @param step The step of the error byte. + */ + void checkErrorByte(const uint8_t errorByte, const uint8_t step); + + std::string makeStepString(const uint8_t step); }; #endif /* MISSION_DEVICES_IMTQHANDLER_H_ */ diff --git a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h index 76dd85c5..a7c32316 100644 --- a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h +++ b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h @@ -13,7 +13,8 @@ namespace IMTQ { static const DeviceCommandId_t GET_CAL_MTM_MEASUREMENT = 0x5; /** Requests the raw values measured by the built-in MTM XEN1210 */ static const DeviceCommandId_t GET_RAW_MTM_MEASUREMENT = 0x6; - static const DeviceCommandId_t SELF_TEST = 0x7; + static const DeviceCommandId_t SELF_TEST_CMD = 0x7; + static const DeviceCommandId_t GET_SELF_TEST_RESULT = 0x8; static const uint8_t GET_TEMP_REPLY_SIZE = 2; static const uint8_t CFGR_CMD_SIZE = 3; @@ -21,6 +22,7 @@ namespace IMTQ { static const uint32_t ENG_HK_DATA_SET_ID = GET_ENG_HK_DATA; static const uint32_t CAL_MTM_SET = GET_CAL_MTM_MEASUREMENT; + static const uint32_t SELF_TEST_DATA_SET = GET_SELF_TEST_RESULT; static const uint8_t SIZE_ENG_HK_COMMAND = 1; static const uint8_t SIZE_STATUS_REPLY = 2; @@ -35,6 +37,16 @@ namespace IMTQ { /** Define entries in IMTQ specific dataset */ static const uint8_t ENG_HK_SET_POOL_ENTRIES = 11; static const uint8_t CAL_MTM_POOL_ENTRIES = 4; + static const uint8_t SELF_TEST_DATASET_ENTRIES = 104; + + /** Error codes for interpreting the self test error byte */ + static const uint8_t I2C_FAILURE_MASK = 0x1; + static const uint8_t SPI_FAILURE_MASK = 0x2; // MTM connectivity + static const uint8_t ADC_FAILURE_MASK = 0x4; // Current/Temp measurement + static const uint8_t PWM_FAILURE_MASK = 0x8; // Coil actuation + static const uint8_t TC_FAILURE_MASK = 0x10; // System failure + static const uint8_t MTM_RANGE_FAILURE_MASK = 0x20; // MTM values outside of expected range + static const uint8_t COIL_CURRENT_FAILURE_MASK = 0x40; // Coil currents outside of expected range /** * Command code definitions. Each command or reply of an IMTQ request will begin with one of @@ -43,13 +55,36 @@ namespace IMTQ { namespace CC { static const uint8_t START_MTM_MEASUREMENT = 0x4; static const uint8_t START_ACTUATION_DIPOLE = 0x6; + static const uint8_t SELF_TEST_CMD = 0x8; static const uint8_t SOFTWARE_RESET = 0xAA; static const uint8_t GET_ENG_HK_DATA = 0x4A; static const uint8_t GET_COMMANDED_DIPOLE = 0x46; static const uint8_t GET_RAW_MTM_MEASUREMENT = 0x42; static const uint8_t GET_CAL_MTM_MEASUREMENT = 0x43; + static const uint8_t GET_SELF_TEST_RESULT = 0x47; }; + namespace SELF_TEST_AXIS { + static const uint8_t ALL = 0x0; + static const uint8_t X_POSITIVE = 0x1; + static const uint8_t X_NEGATIVE = 0x2; + static const uint8_t Y_POSITIVE = 0x3; + static const uint8_t Y_NEGATIVE = 0x4; + static const uint8_t Z_POSITIVE = 0x5; + static const uint8_t Z_NEGATIVE = 0x6; + } + + namespace SELF_TEST_STEPS { + static const uint8_t INIT = 0x0; + static const uint8_t X_POSITIVE = 0x1; + static const uint8_t X_NEGATIVE = 0x2; + static const uint8_t Y_POSITIVE = 0x3; + static const uint8_t Y_NEGATIVE = 0x4; + static const uint8_t Z_POSITIVE = 0x5; + static const uint8_t Z_NEGATIVE = 0x6; + static const uint8_t FINA = 0x7; + } + enum IMTQPoolIds: lp_id_t { DIGITAL_VOLTAGE_MV, ANALOG_VOLTAGE_MV, @@ -69,98 +104,394 @@ namespace IMTQ { MTM_RAW_X, MTM_RAW_Y, MTM_RAW_Z, - ACTUATION_RAW_STATUS + ACTUATION_RAW_STATUS, + + INIT_ERR, + INIT_RAW_MAG_X, + INIT_RAW_MAG_Y, + INIT_RAW_MAG_Z, + INIT_CAL_MAG_X, + INIT_CAL_MAG_Y, + INIT_CAL_MAG_Z, + INIT_COIL_X_CURRENT, + INIT_COIL_Y_CURRENT, + INIT_COIL_Z_CURRENT, + INIT_COIL_X_TEMPERATURE, + INIT_COIL_Y_TEMPERATURE, + INIT_COIL_Z_TEMPERATURE, + + POS_X_ERR, + POS_X_RAW_MAG_X, + POS_X_RAW_MAG_Y, + POS_X_RAW_MAG_Z, + POS_X_CAL_MAG_X, + POS_X_CAL_MAG_Y, + POS_X_CAL_MAG_Z, + POS_X_COIL_X_CURRENT, + POS_X_COIL_Y_CURRENT, + POS_X_COIL_Z_CURRENT, + POS_X_COIL_X_TEMPERATURE, + POS_X_COIL_Y_TEMPERATURE, + POS_X_COIL_Z_TEMPERATURE, + + NEG_X_ERR, + NEG_X_RAW_MAG_X, + NEG_X_RAW_MAG_Y, + NEG_X_RAW_MAG_Z, + NEG_X_CAL_MAG_X, + NEG_X_CAL_MAG_Y, + NEG_X_CAL_MAG_Z, + NEG_X_COIL_X_CURRENT, + NEG_X_COIL_Y_CURRENT, + NEG_X_COIL_Z_CURRENT, + NEG_X_COIL_X_TEMPERATURE, + NEG_X_COIL_Y_TEMPERATURE, + NEG_X_COIL_Z_TEMPERATURE, + + POS_Y_ERR, + POS_Y_RAW_MAG_X, + POS_Y_RAW_MAG_Y, + POS_Y_RAW_MAG_Z, + POS_Y_CAL_MAG_X, + POS_Y_CAL_MAG_Y, + POS_Y_CAL_MAG_Z, + POS_Y_COIL_X_CURRENT, + POS_Y_COIL_Y_CURRENT, + POS_Y_COIL_Z_CURRENT, + POS_Y_COIL_X_TEMPERATURE, + POS_Y_COIL_Y_TEMPERATURE, + POS_Y_COIL_Z_TEMPERATURE, + + NEG_Y_ERR, + NEG_Y_RAW_MAG_X, + NEG_Y_RAW_MAG_Y, + NEG_Y_RAW_MAG_Z, + NEG_Y_CAL_MAG_X, + NEG_Y_CAL_MAG_Y, + NEG_Y_CAL_MAG_Z, + NEG_Y_COIL_X_CURRENT, + NEG_Y_COIL_Y_CURRENT, + NEG_Y_COIL_Z_CURRENT, + NEG_Y_COIL_X_TEMPERATURE, + NEG_Y_COIL_Y_TEMPERATURE, + NEG_Y_COIL_Z_TEMPERATURE, + + POS_Z_ERR, + POS_Z_RAW_MAG_X, + POS_Z_RAW_MAG_Y, + POS_Z_RAW_MAG_Z, + POS_Z_CAL_MAG_X, + POS_Z_CAL_MAG_Y, + POS_Z_CAL_MAG_Z, + POS_Z_COIL_X_CURRENT, + POS_Z_COIL_Y_CURRENT, + POS_Z_COIL_Z_CURRENT, + POS_Z_COIL_X_TEMPERATURE, + POS_Z_COIL_Y_TEMPERATURE, + POS_Z_COIL_Z_TEMPERATURE, + + NEG_Z_ERR, + NEG_Z_RAW_MAG_X, + NEG_Z_RAW_MAG_Y, + NEG_Z_RAW_MAG_Z, + NEG_Z_CAL_MAG_X, + NEG_Z_CAL_MAG_Y, + NEG_Z_CAL_MAG_Z, + NEG_Z_COIL_X_CURRENT, + NEG_Z_COIL_Y_CURRENT, + NEG_Z_COIL_Z_CURRENT, + NEG_Z_COIL_X_TEMPERATURE, + NEG_Z_COIL_Y_TEMPERATURE, + NEG_Z_COIL_Z_TEMPERATURE, + + FINA_ERR, + FINA_RAW_MAG_X, + FINA_RAW_MAG_Y, + FINA_RAW_MAG_Z, + FINA_CAL_MAG_X, + FINA_CAL_MAG_Y, + FINA_CAL_MAG_Z, + FINA_COIL_X_CURRENT, + FINA_COIL_Y_CURRENT, + FINA_COIL_Z_CURRENT, + FINA_COIL_X_TEMPERATURE, + FINA_COIL_Y_TEMPERATURE, + FINA_COIL_Z_TEMPERATURE, }; -class EngHkDataset: - public StaticLocalDataSet { +class EngHkDataset: public StaticLocalDataSet { public: - EngHkDataset(HasLocalDataPoolIF* owner): - StaticLocalDataSet(owner, ENG_HK_DATA_SET_ID) { - } + EngHkDataset(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, ENG_HK_DATA_SET_ID) { + } - EngHkDataset(object_id_t objectId): - StaticLocalDataSet(sid_t(objectId, ENG_HK_DATA_SET_ID)) { - } + EngHkDataset(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, ENG_HK_DATA_SET_ID)) { + } - lp_var_t digitalVoltageMv = lp_var_t(sid.objectId, - DIGITAL_VOLTAGE_MV, this); - lp_var_t analogVoltageMv = lp_var_t(sid.objectId, - ANALOG_VOLTAGE_MV, this); - lp_var_t digitalCurrentmA = lp_var_t(sid.objectId, - DIGITAL_CURRENT, this); - lp_var_t analogCurrentmA = lp_var_t(sid.objectId, - ANALOG_CURRENT, this); - lp_var_t coilXCurrentmA = lp_var_t(sid.objectId, - COIL_X_CURRENT, this); - lp_var_t coilYCurrentmA = lp_var_t(sid.objectId, - COIL_Y_CURRENT, this); - lp_var_t coilZCurrentmA = lp_var_t(sid.objectId, - COIL_Z_CURRENT, this); - /** All temperatures in [�C] */ - lp_var_t coilXTemperature = lp_var_t(sid.objectId, - COIL_X_TEMPERATURE, this); - lp_var_t coilYTemperature = lp_var_t(sid.objectId, - COIL_Y_TEMPERATURE, this); - lp_var_t coilZTemperature = lp_var_t(sid.objectId, - COIL_Z_TEMPERATURE, this); - lp_var_t mcuTemperature = lp_var_t(sid.objectId, - MCU_TEMPERATURE, this); + lp_var_t digitalVoltageMv = lp_var_t(sid.objectId, DIGITAL_VOLTAGE_MV, + this); + lp_var_t analogVoltageMv = lp_var_t(sid.objectId, ANALOG_VOLTAGE_MV, this); + lp_var_t digitalCurrentmA = lp_var_t(sid.objectId, DIGITAL_CURRENT, this); + lp_var_t analogCurrentmA = lp_var_t(sid.objectId, ANALOG_CURRENT, this); + lp_var_t coilXCurrentmA = lp_var_t(sid.objectId, COIL_X_CURRENT, this); + lp_var_t coilYCurrentmA = lp_var_t(sid.objectId, COIL_Y_CURRENT, this); + lp_var_t coilZCurrentmA = lp_var_t(sid.objectId, COIL_Z_CURRENT, this); + /** All temperatures in [°C] */ + lp_var_t coilXTemperature = lp_var_t(sid.objectId, COIL_X_TEMPERATURE, + this); + lp_var_t coilYTemperature = lp_var_t(sid.objectId, COIL_Y_TEMPERATURE, + this); + lp_var_t coilZTemperature = lp_var_t(sid.objectId, COIL_Z_TEMPERATURE, + this); + lp_var_t mcuTemperature = lp_var_t(sid.objectId, MCU_TEMPERATURE, this); }; /** * @brief This dataset holds the raw MTM measurements. */ -class CalibratedMtmMeasurementSet: - public StaticLocalDataSet { +class CalibratedMtmMeasurementSet: public StaticLocalDataSet { public: - CalibratedMtmMeasurementSet(HasLocalDataPoolIF* owner): - StaticLocalDataSet(owner, CAL_MTM_SET) { + CalibratedMtmMeasurementSet(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, CAL_MTM_SET) { } - CalibratedMtmMeasurementSet(object_id_t objectId): - StaticLocalDataSet(sid_t(objectId, CAL_MTM_SET)) { + CalibratedMtmMeasurementSet(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, CAL_MTM_SET)) { } /** The unit of all measurements is nT */ - lp_var_t mtmXnT = lp_var_t(sid.objectId, - MTM_CAL_X, this); - lp_var_t mtmYnT = lp_var_t(sid.objectId, - MTM_CAL_Y, this); - lp_var_t mtmZnT = lp_var_t(sid.objectId, - MTM_CAL_Z, this); + lp_var_t mtmXnT = lp_var_t(sid.objectId, MTM_CAL_X, this); + lp_var_t mtmYnT = lp_var_t(sid.objectId, MTM_CAL_Y, this); + lp_var_t mtmZnT = lp_var_t(sid.objectId, MTM_CAL_Z, this); /** 1 if coils were actuating during measurement otherwise 0 */ - lp_var_t coilActuationStatus = lp_var_t(sid.objectId, - ACTUATION_CAL_STATUS, this); + lp_var_t coilActuationStatus = lp_var_t(sid.objectId, ACTUATION_CAL_STATUS, + this); }; /** * @brief This dataset holds the last calibrated MTM measurement. */ -class RawMtmMeasurementSet: - public StaticLocalDataSet { +class RawMtmMeasurementSet: public StaticLocalDataSet { public: - RawMtmMeasurementSet(HasLocalDataPoolIF* owner): - StaticLocalDataSet(owner, CAL_MTM_SET) { + RawMtmMeasurementSet(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, CAL_MTM_SET) { } - RawMtmMeasurementSet(object_id_t objectId): - StaticLocalDataSet(sid_t(objectId, CAL_MTM_SET)) { + RawMtmMeasurementSet(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, CAL_MTM_SET)) { } /** The unit of all measurements is nT */ - lp_var_t mtmXnT = lp_var_t(sid.objectId, - MTM_RAW_X, this); - lp_var_t mtmYnT = lp_var_t(sid.objectId, - MTM_RAW_Y, this); - lp_var_t mtmZnT = lp_var_t(sid.objectId, - MTM_RAW_Z, this); + lp_var_t mtmXnT = lp_var_t(sid.objectId, MTM_RAW_X, this); + lp_var_t mtmYnT = lp_var_t(sid.objectId, MTM_RAW_Y, this); + lp_var_t mtmZnT = lp_var_t(sid.objectId, MTM_RAW_Z, this); /** 1 if coils were actuating during measurement otherwise 0 */ - lp_var_t coilActuationStatus = lp_var_t(sid.objectId, - ACTUATION_RAW_STATUS, this); + lp_var_t coilActuationStatus = lp_var_t(sid.objectId, ACTUATION_RAW_STATUS, + this); +}; + +/** + * @brief This dataset can be used to store the self test results. + * + * @details Units of measurements: + * Currents: [10^-4 A] + * Raw MTM data: [7.5 * 10^-9 T] + * Calibrated MTM data: [10^-9 T] + * Temperature: [°C] + * The self test generates for each axis the positive and negative dipole and measures + * the magnetic field with the built-in MTM. The procedure of the test is as follows: + * 1. All coils off (INIT step) + * 2. +X + * 3. -X + * 4. +Y + * 5. -Y + * 6. +Z + * 7. -Z + * 8. All coils off (FINA step) + */ +class SelfTestDataset: public StaticLocalDataSet { +public: + + SelfTestDataset(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, SELF_TEST_DATA_SET) { + } + + SelfTestDataset(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, SELF_TEST_DATA_SET)) { + } + + /** INIT block */ + lp_var_t initErr = lp_var_t(sid.objectId, INIT_ERR, this); + lp_var_t initRawMagX = lp_var_t(sid.objectId, INIT_RAW_MAG_X, this); + lp_var_t initRawMagY = lp_var_t(sid.objectId, INIT_RAW_MAG_Y, this); + lp_var_t initRawMagZ = lp_var_t(sid.objectId, INIT_RAW_MAG_Z, this); + lp_var_t initCalMagX = lp_var_t(sid.objectId, INIT_CAL_MAG_X, this); + lp_var_t initCalMagY = lp_var_t(sid.objectId, INIT_CAL_MAG_Y, this); + lp_var_t initCalMagZ = lp_var_t(sid.objectId, INIT_CAL_MAG_Z, this); + lp_var_t initCoilXCurrent = lp_var_t(sid.objectId, INIT_COIL_X_CURRENT, + this); + lp_var_t initCoilYCurrent = lp_var_t(sid.objectId, INIT_COIL_Y_CURRENT, + this); + lp_var_t initCoilZCurrent = lp_var_t(sid.objectId, INIT_COIL_Z_CURRENT, + this); + lp_var_t initCoilXTemperature = lp_var_t(sid.objectId, + INIT_COIL_X_TEMPERATURE, this); + lp_var_t initCoilYTemperature = lp_var_t(sid.objectId, + INIT_COIL_Y_TEMPERATURE, this); + lp_var_t initCoilZTemperature = lp_var_t(sid.objectId, + INIT_COIL_Z_TEMPERATURE, this); + + /** +X block */ + lp_var_t posXErr = lp_var_t(sid.objectId, POS_X_ERR, this); + lp_var_t posXRawMagX = lp_var_t(sid.objectId, POS_X_RAW_MAG_X, this); + lp_var_t posXRawMagY = lp_var_t(sid.objectId, POS_X_RAW_MAG_Y, this); + lp_var_t posXRawMagZ = lp_var_t(sid.objectId, POS_X_RAW_MAG_Z, this); + lp_var_t posXCalMagX = lp_var_t(sid.objectId, POS_X_CAL_MAG_X, this); + lp_var_t posXCalMagY = lp_var_t(sid.objectId, POS_X_CAL_MAG_Y, this); + lp_var_t posXCalMagZ = lp_var_t(sid.objectId, POS_X_CAL_MAG_Z, this); + lp_var_t posXCoilXCurrent = lp_var_t(sid.objectId, POS_X_COIL_X_CURRENT, + this); + lp_var_t posXCoilYCurrent = lp_var_t(sid.objectId, POS_X_COIL_Y_CURRENT, + this); + lp_var_t posXCoilZCurrent = lp_var_t(sid.objectId, POS_X_COIL_Z_CURRENT, + this); + lp_var_t posXCoilXTemperature = lp_var_t(sid.objectId, + POS_X_COIL_X_TEMPERATURE, this); + lp_var_t posXCoilYTemperature = lp_var_t(sid.objectId, + POS_X_COIL_Y_TEMPERATURE, this); + lp_var_t posXCoilZTemperature = lp_var_t(sid.objectId, + POS_X_COIL_Z_TEMPERATURE, this); + + /** -X block */ + lp_var_t negXErr = lp_var_t(sid.objectId, NEG_X_ERR, this); + lp_var_t negXRawMagX = lp_var_t(sid.objectId, NEG_X_RAW_MAG_X, this); + lp_var_t negXRawMagY = lp_var_t(sid.objectId, NEG_X_RAW_MAG_Y, this); + lp_var_t negXRawMagZ = lp_var_t(sid.objectId, NEG_X_RAW_MAG_Z, this); + lp_var_t negXCalMagX = lp_var_t(sid.objectId, NEG_X_CAL_MAG_X, this); + lp_var_t negXCalMagY = lp_var_t(sid.objectId, NEG_X_CAL_MAG_Y, this); + lp_var_t negXCalMagZ = lp_var_t(sid.objectId, NEG_X_CAL_MAG_Z, this); + lp_var_t negXCoilXCurrent = lp_var_t(sid.objectId, NEG_X_COIL_X_CURRENT, + this); + lp_var_t negXCoilYCurrent = lp_var_t(sid.objectId, NEG_X_COIL_Y_CURRENT, + this); + lp_var_t negXCoilZCurrent = lp_var_t(sid.objectId, NEG_X_COIL_Z_CURRENT, + this); + lp_var_t negXCoilXTemperature = lp_var_t(sid.objectId, + NEG_X_COIL_X_TEMPERATURE, this); + lp_var_t negXCoilYTemperature = lp_var_t(sid.objectId, + NEG_X_COIL_Y_TEMPERATURE, this); + lp_var_t negXCoilZTemperature = lp_var_t(sid.objectId, + NEG_X_COIL_Z_TEMPERATURE, this); + + /** +Y block */ + lp_var_t posYErr = lp_var_t(sid.objectId, POS_Y_ERR, this); + lp_var_t posYRawMagX = lp_var_t(sid.objectId, POS_Y_RAW_MAG_X, this); + lp_var_t posYRawMagY = lp_var_t(sid.objectId, POS_Y_RAW_MAG_Y, this); + lp_var_t posYRawMagZ = lp_var_t(sid.objectId, POS_Y_RAW_MAG_Z, this); + lp_var_t posYCalMagX = lp_var_t(sid.objectId, POS_Y_CAL_MAG_X, this); + lp_var_t posYCalMagY = lp_var_t(sid.objectId, POS_Y_CAL_MAG_Y, this); + lp_var_t posYCalMagZ = lp_var_t(sid.objectId, POS_Y_CAL_MAG_Z, this); + lp_var_t posYCoilXCurrent = lp_var_t(sid.objectId, POS_Y_COIL_X_CURRENT, + this); + lp_var_t posYCoilYCurrent = lp_var_t(sid.objectId, POS_Y_COIL_Y_CURRENT, + this); + lp_var_t posYCoilZCurrent = lp_var_t(sid.objectId, POS_Y_COIL_Z_CURRENT, + this); + lp_var_t posYCoilXTemperature = lp_var_t(sid.objectId, + POS_Y_COIL_X_TEMPERATURE, this); + lp_var_t posYCoilYTemperature = lp_var_t(sid.objectId, + POS_Y_COIL_Y_TEMPERATURE, this); + lp_var_t posYCoilZTemperature = lp_var_t(sid.objectId, + POS_Y_COIL_Z_TEMPERATURE, this); + + /** -Y block */ + lp_var_t negYErr = lp_var_t(sid.objectId, NEG_Y_ERR, this); + lp_var_t negYRawMagX = lp_var_t(sid.objectId, NEG_Y_RAW_MAG_X, this); + lp_var_t negYRawMagY = lp_var_t(sid.objectId, NEG_Y_RAW_MAG_Y, this); + lp_var_t negYRawMagZ = lp_var_t(sid.objectId, NEG_Y_RAW_MAG_Z, this); + lp_var_t negYCalMagX = lp_var_t(sid.objectId, NEG_Y_CAL_MAG_X, this); + lp_var_t negYCalMagY = lp_var_t(sid.objectId, NEG_Y_CAL_MAG_Y, this); + lp_var_t negYCalMagZ = lp_var_t(sid.objectId, NEG_Y_CAL_MAG_Z, this); + lp_var_t negYCoilXCurrent = lp_var_t(sid.objectId, NEG_Y_COIL_X_CURRENT, + this); + lp_var_t negYCoilYCurrent = lp_var_t(sid.objectId, NEG_Y_COIL_Y_CURRENT, + this); + lp_var_t negYCoilZCurrent = lp_var_t(sid.objectId, NEG_Y_COIL_Z_CURRENT, + this); + lp_var_t negYCoilXTemperature = lp_var_t(sid.objectId, + NEG_Y_COIL_X_TEMPERATURE, this); + lp_var_t negYCoilYTemperature = lp_var_t(sid.objectId, + NEG_Y_COIL_Y_TEMPERATURE, this); + lp_var_t negYCoilZTemperature = lp_var_t(sid.objectId, + NEG_Y_COIL_Z_TEMPERATURE, this); + + /** +Z block */ + lp_var_t posZErr = lp_var_t(sid.objectId, POS_Z_ERR, this); + lp_var_t posZRawMagX = lp_var_t(sid.objectId, POS_Z_RAW_MAG_X, this); + lp_var_t posZRawMagY = lp_var_t(sid.objectId, POS_Z_RAW_MAG_Y, this); + lp_var_t posZRawMagZ = lp_var_t(sid.objectId, POS_Z_RAW_MAG_Z, this); + lp_var_t posZCalMagX = lp_var_t(sid.objectId, POS_Z_CAL_MAG_X, this); + lp_var_t posZCalMagY = lp_var_t(sid.objectId, POS_Z_CAL_MAG_Y, this); + lp_var_t posZCalMagZ = lp_var_t(sid.objectId, POS_Z_CAL_MAG_Z, this); + lp_var_t posZCoilXCurrent = lp_var_t(sid.objectId, POS_Z_COIL_X_CURRENT, + this); + lp_var_t posZCoilYCurrent = lp_var_t(sid.objectId, POS_Z_COIL_Y_CURRENT, + this); + lp_var_t posZCoilZCurrent = lp_var_t(sid.objectId, POS_Z_COIL_Z_CURRENT, + this); + lp_var_t posZCoilXTemperature = lp_var_t(sid.objectId, + POS_Z_COIL_X_TEMPERATURE, this); + lp_var_t posZCoilYTemperature = lp_var_t(sid.objectId, + POS_Z_COIL_Y_TEMPERATURE, this); + lp_var_t posZCoilZTemperature = lp_var_t(sid.objectId, + POS_Z_COIL_Z_TEMPERATURE, this); + + /** -Z block */ + lp_var_t negZErr = lp_var_t(sid.objectId, NEG_Z_ERR, this); + lp_var_t negZRawMagX = lp_var_t(sid.objectId, NEG_Z_RAW_MAG_X, this); + lp_var_t negZRawMagY = lp_var_t(sid.objectId, NEG_Z_RAW_MAG_Y, this); + lp_var_t negZRawMagZ = lp_var_t(sid.objectId, NEG_Z_RAW_MAG_Z, this); + lp_var_t negZCalMagX = lp_var_t(sid.objectId, NEG_Z_CAL_MAG_X, this); + lp_var_t negZCalMagY = lp_var_t(sid.objectId, NEG_Z_CAL_MAG_Y, this); + lp_var_t negZCalMagZ = lp_var_t(sid.objectId, NEG_Z_CAL_MAG_Z, this); + lp_var_t negZCoilXCurrent = lp_var_t(sid.objectId, NEG_Z_COIL_X_CURRENT, + this); + lp_var_t negZCoilYCurrent = lp_var_t(sid.objectId, NEG_Z_COIL_Y_CURRENT, + this); + lp_var_t negZCoilZCurrent = lp_var_t(sid.objectId, NEG_Z_COIL_Z_CURRENT, + this); + lp_var_t negZCoilXTemperature = lp_var_t(sid.objectId, + NEG_Z_COIL_X_TEMPERATURE, this); + lp_var_t negZCoilYTemperature = lp_var_t(sid.objectId, + NEG_Z_COIL_Y_TEMPERATURE, this); + lp_var_t negZCoilZTemperature = lp_var_t(sid.objectId, + NEG_Z_COIL_Z_TEMPERATURE, this); + + /** FINA block */ + lp_var_t finaErr = lp_var_t(sid.objectId, FINA_ERR, this); + lp_var_t finaRawMagX = lp_var_t(sid.objectId, FINA_RAW_MAG_X, this); + lp_var_t finaRawMagY = lp_var_t(sid.objectId, FINA_RAW_MAG_Y, this); + lp_var_t finaRawMagZ = lp_var_t(sid.objectId, FINA_RAW_MAG_Z, this); + lp_var_t finaCalMagX = lp_var_t(sid.objectId, FINA_CAL_MAG_X, this); + lp_var_t finaCalMagY = lp_var_t(sid.objectId, FINA_CAL_MAG_Y, this); + lp_var_t finaCalMagZ = lp_var_t(sid.objectId, FINA_CAL_MAG_Z, this); + lp_var_t finaCoilXCurrent = lp_var_t(sid.objectId, FINA_COIL_X_CURRENT, + this); + lp_var_t finaCoilYCurrent = lp_var_t(sid.objectId, FINA_COIL_Y_CURRENT, + this); + lp_var_t finaCoilZCurrent = lp_var_t(sid.objectId, FINA_COIL_Z_CURRENT, + this); + lp_var_t finaCoilXTemperature = lp_var_t(sid.objectId, + FINA_COIL_X_TEMPERATURE, this); + lp_var_t finaCoilYTemperature = lp_var_t(sid.objectId, + FINA_COIL_Y_TEMPERATURE, this); + lp_var_t finaCoilZTemperature = lp_var_t(sid.objectId, + FINA_COIL_Z_TEMPERATURE, this); + }; /**