diff --git a/mission/devices/IMTQHandler.cpp b/mission/devices/IMTQHandler.cpp index 5d4e7d1d..4c590b76 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), selfTestDataset(this) { + this), rawMtmMeasurementSet(this), posXselfTestDataset(this) { if (comCookie == NULL) { sif::error << "IMTQHandler: Invalid com cookie" << std::endl; } @@ -18,14 +18,11 @@ IMTQHandler::~IMTQHandler() { void IMTQHandler::doStartUp(){ - if(selfTestPerformed == true){ #if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1 setMode(MODE_NORMAL); #else setMode(_MODE_TO_ON); #endif - selfTestPerformed = false; - } } void IMTQHandler::doShutDown(){ @@ -61,31 +58,16 @@ 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; - startupStep = StartupStep::GET_SELF_TEST_RESULT; - break; - case StartupStep::GET_SELF_TEST_RESULT: - *id = IMTQ::GET_SELF_TEST_RESULT; - startupStep = StartupStep::NONE; - break; - default: - sif::debug << "IMTQHandler::buildTransitionDeviceCommand: Unknown startup step" - << std::endl; - } - } - return buildCommandFromCommand(*id, nullptr, 0);; + return RETURN_OK; } ReturnValue_t IMTQHandler::buildCommandFromCommand( DeviceCommandId_t deviceCommand, const uint8_t * commandData, size_t commandDataLen) { switch(deviceCommand) { - case(IMTQ::SELF_TEST_CMD): { + case(IMTQ::POS_X_SELF_TEST): { commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; - commandBuffer[1] = IMTQ::SELF_TEST_AXIS::ALL; + commandBuffer[1] = IMTQ::SELF_TEST_AXIS::X_POSITIVE; rawPacket = commandBuffer; rawPacketLen = 2; return RETURN_OK; @@ -148,7 +130,17 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand( } void IMTQHandler::fillCommandAndReplyMap() { - this->insertInCommandAndReplyMap(IMTQ::SELF_TEST_CMD, 1, nullptr, + 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); @@ -198,8 +190,8 @@ ReturnValue_t IMTQHandler::scanForReply(const uint8_t *start, break; case(IMTQ::CC::SELF_TEST_CMD): *foundLen = IMTQ::SIZE_STATUS_REPLY; - *foundId = IMTQ::SELF_TEST_CMD; - break; + result = getSelfTestCommandId(foundId); + break; case(IMTQ::CC::GET_SELF_TEST_RESULT): *foundLen = IMTQ::SIZE_SELF_TEST_RESULTS; *foundId = IMTQ::GET_SELF_TEST_RESULT; @@ -225,7 +217,12 @@ ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, } switch (id) { - case (IMTQ::SELF_TEST_CMD): + 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 */ @@ -290,20 +287,20 @@ 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 })); + /** Entries of dataset for self test results of 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 })); localDataPoolMap.emplace(IMTQ::POS_X_ERR, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_X_RAW_MAG_X, new PoolEntry( { 0 })); @@ -319,6 +316,20 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat 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::INIT_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 })); + 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 })); @@ -389,23 +400,28 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat 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; } +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 RETURN_OK; +} + ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) { uint8_t cmdErrorField = *(packet + 1) & 0xF; switch (cmdErrorField) { @@ -575,506 +591,209 @@ void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) { } void IMTQHandler::handleSelfTestReply(const uint8_t* packet) { - PoolReadGuard rg(&selfTestDataset); - - /** Init measurements */ - int8_t offset = 2; + uint16_t offset = 2; checkErrorByte(*(packet + offset), *(packet + offset + 1)); - selfTestDataset.initErr = *(packet + offset); + + switch(*(packet + IMTQ::MAIN_STEP_OFFSET)) { + case IMTQ::SELF_TEST_STEPS::X_POSITIVE: { + handlePositiveXSelfTestReply(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 - selfTestDataset.initRawMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + posXselfTestDataset.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 + posXselfTestDataset.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 + posXselfTestDataset.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 + posXselfTestDataset.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 + posXselfTestDataset.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 + posXselfTestDataset.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); + posXselfTestDataset.initCoilXCurrent = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; - selfTestDataset.initCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + posXselfTestDataset.initCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; - selfTestDataset.initCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + posXselfTestDataset.initCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; - selfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + posXselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; - selfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + posXselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; - selfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + posXselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; /** +X measurements */ checkErrorByte(*(packet + offset), *(packet + offset + 1)); - selfTestDataset.posXErr = *(packet + offset); + posXselfTestDataset.err = *(packet + offset); offset += 2; // STEP byte will not be stored - selfTestDataset.posXRawMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + posXselfTestDataset.rawMagX = *(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 + posXselfTestDataset.rawMagY = *(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 + posXselfTestDataset.rawMagZ = *(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 + posXselfTestDataset.calMagX = *(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 + posXselfTestDataset.calMagY = *(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 + posXselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; - selfTestDataset.posXCoilXCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + posXselfTestDataset.coilXCurrent = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; - selfTestDataset.posXCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + posXselfTestDataset.coilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; - selfTestDataset.posXCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + posXselfTestDataset.coilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; - selfTestDataset.posXCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + posXselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; - selfTestDataset.posXCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + posXselfTestDataset.coilYTemperature = *(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); + posXselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; /** FINA measurements */ - selfTestDataset.finaErr = *(packet + offset); + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + posXselfTestDataset.finaErr = *(packet + offset); offset += 2; // STEP byte will not be stored - selfTestDataset.finaRawMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + posXselfTestDataset.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 + posXselfTestDataset.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 + posXselfTestDataset.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 + posXselfTestDataset.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 + posXselfTestDataset.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 + posXselfTestDataset.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); + posXselfTestDataset.finaCoilXCurrent = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; - selfTestDataset.finaCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + posXselfTestDataset.finaCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; - selfTestDataset.finaCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset); + posXselfTestDataset.finaCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; - selfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + posXselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; - selfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + posXselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; - selfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + posXselfTestDataset.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 + << static_cast(posXselfTestDataset.initErr.value) << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field X: " << posXselfTestDataset.initRawMagX << " 7.5 * 10^-9 T" << std::endl; - sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << selfTestDataset.initRawMagY + sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << posXselfTestDataset.initRawMagY << " 7.5 * 10^-9 T" << std::endl; - sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << selfTestDataset.initRawMagZ + sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << posXselfTestDataset.initRawMagZ << " 7.5 * 10^-9 T" << std::endl; - sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " << selfTestDataset.initCalMagX + sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " << posXselfTestDataset.initCalMagX << " 10^-9 T" << std::endl; - sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " << selfTestDataset.initCalMagY + sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " << posXselfTestDataset.initCalMagY << " 10^-9 T" << std::endl; - sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " << selfTestDataset.initCalMagZ + sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " << posXselfTestDataset.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 current: " << posXselfTestDataset.initCoilXCurrent + << " * 10^-4 A" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y current: " << posXselfTestDataset.initCoilYCurrent + << " * 10^-4 A" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z current: " << posXselfTestDataset.initCoilZCurrent + << " * 10^-4 A" << std::endl; sif::info << "IMTQ self test (INIT) coil X temperature: " - << selfTestDataset.initCoilXTemperature << " °C" << std::endl; + << posXselfTestDataset.initCoilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (INIT) coil Y temperature: " - << selfTestDataset.initCoilYTemperature << " °C" << std::endl; + << posXselfTestDataset.initCoilYTemperature << " °C" << std::endl; sif::info << "IMTQ self test (INIT) coil Z temperature: " - << selfTestDataset.initCoilZTemperature << " °C" << std::endl; + << posXselfTestDataset.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 + static_cast(posXselfTestDataset.err.value) << std::endl; + sif::info << "IMTQ self test (+X) raw magnetic field X: " << posXselfTestDataset.rawMagX << " 7.5 * 10^-9 T" << std::endl; - sif::info << "IMTQ self test (+X) raw magnetic field Y: " << selfTestDataset.posXRawMagY + sif::info << "IMTQ self test (+X) raw magnetic field Y: " << posXselfTestDataset.rawMagY << " 7.5 * 10^-9 T" << std::endl; - sif::info << "IMTQ self test (+X) raw magnetic field Z: " << selfTestDataset.posXRawMagZ + sif::info << "IMTQ self test (+X) raw magnetic field Z: " << posXselfTestDataset.rawMagZ << " 7.5 * 10^-9 T" << std::endl; - sif::info << "IMTQ self test (+X) calibrated magnetic field X: " << selfTestDataset.posXCalMagX + sif::info << "IMTQ self test (+X) calibrated magnetic field X: " << posXselfTestDataset.calMagX << " 10^-9 T" << std::endl; - sif::info << "IMTQ self test (+X) calibrated magnetic field Y: " << selfTestDataset.posXCalMagY + sif::info << "IMTQ self test (+X) calibrated magnetic field Y: " << posXselfTestDataset.calMagY << " 10^-9 T" << std::endl; - sif::info << "IMTQ self test (+X) calibrated magnetic field Z: " << selfTestDataset.posXCalMagZ + sif::info << "IMTQ self test (+X) calibrated magnetic field Z: " << posXselfTestDataset.calMagZ << " 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 current: " << posXselfTestDataset.coilXCurrent + << " * 10^-4 A" << std::endl; + sif::info << "IMTQ self test (+X) coil Y current: " << posXselfTestDataset.coilYCurrent + << " * 10^-4 A" << std::endl; + sif::info << "IMTQ self test (+X) coil Z current: " << posXselfTestDataset.coilZCurrent + << " * 10^-4 A" << std::endl; sif::info << "IMTQ self test (+X) coil X temperature: " - << selfTestDataset.posXCoilXTemperature << " °C" << std::endl; + << posXselfTestDataset.coilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (+X) coil Y temperature: " - << selfTestDataset.posXCoilYTemperature << " °C" << std::endl; + << posXselfTestDataset.coilYTemperature << " °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; + << posXselfTestDataset.coilZTemperature << " °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 + << static_cast(posXselfTestDataset.finaErr.value) << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field X: " << posXselfTestDataset.finaRawMagX << " 7.5 * 10^-9 T" << std::endl; - sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << selfTestDataset.finaRawMagY + sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << posXselfTestDataset.finaRawMagY << " 7.5 * 10^-9 T" << std::endl; - sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << selfTestDataset.finaRawMagZ + sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << posXselfTestDataset.finaRawMagZ << " 7.5 * 10^-9 T" << std::endl; - sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " << selfTestDataset.finaCalMagX + sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " << posXselfTestDataset.finaCalMagX << " 10^-9 T" << std::endl; - sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " << selfTestDataset.finaCalMagY + sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " << posXselfTestDataset.finaCalMagY << " 10^-9 T" << std::endl; - sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " << selfTestDataset.finaCalMagZ + sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " << posXselfTestDataset.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 current: " << posXselfTestDataset.finaCoilXCurrent + << " * 10^-4 A" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y current: " << posXselfTestDataset.finaCoilYCurrent + << " * 10^-4 A" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z current: " << posXselfTestDataset.finaCoilZCurrent + << " * 10^-4 A" << std::endl; sif::info << "IMTQ self test (FINA) coil X temperature: " - << selfTestDataset.finaCoilXTemperature << " °C" << std::endl; + << posXselfTestDataset.finaCoilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (FINA) coil Y temperature: " - << selfTestDataset.finaCoilYTemperature << " °C" << std::endl; + << posXselfTestDataset.finaCoilYTemperature << " °C" << std::endl; sif::info << "IMTQ self test (FINA) coil Z temperature: " - << selfTestDataset.finaCoilZTemperature << " °C" << std::endl; + << posXselfTestDataset.finaCoilZTemperature << " °C" << std::endl; #endif - if (mode == _MODE_START_UP) { - selfTestPerformed = true; - } } void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) { @@ -1088,6 +807,10 @@ void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) { 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 @@ -1123,6 +846,12 @@ void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t 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) { diff --git a/mission/devices/IMTQHandler.h b/mission/devices/IMTQHandler.h index dd5ae8cf..2522cfe5 100644 --- a/mission/devices/IMTQHandler.h +++ b/mission/devices/IMTQHandler.h @@ -49,6 +49,8 @@ private: static const ReturnValue_t INTERNAL_PROCESSING_ERROR = MAKE_RETURN_CODE(0xA4); static const ReturnValue_t REJECTED_WITHOUT_REASON = MAKE_RETURN_CODE(0xA5); static const ReturnValue_t CMD_ERR_UNKNOWN = MAKE_RETURN_CODE(0xA6); + //! [EXPORT] : [COMMENT] The status reply to a self test command was received but no self test command has been sent. This should normally never happen. + static const ReturnValue_t UNEXPECTED_SELF_TEST_REPLY = MAKE_RETURN_CODE(0xA7); static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::IMTQ_HANDLER; @@ -73,11 +75,13 @@ private: //! [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); + //! [EXPORT] : [COMMENT] Received invalid error byte. This indicates an error of the communication link between IMTQ and OBC. + static const Event INVALID_ERROR_BYTE = MAKE_EVENT(8, severity::LOW); IMTQ::EngHkDataset engHkDataset; IMTQ::CalibratedMtmMeasurementSet calMtmMeasurementSet; IMTQ::RawMtmMeasurementSet rawMtmMeasurementSet; - IMTQ::SelfTestDataset selfTestDataset; + IMTQ::PosXselfTestSet posXselfTestDataset; uint8_t commandBuffer[IMTQ::MAX_COMMAND_SIZE]; @@ -100,6 +104,12 @@ private: bool selfTestPerformed = false; + /** + * @brief In case of a status reply to a single axis self test command, this function + * searches for the actual pending command. + */ + ReturnValue_t getSelfTestCommandId(DeviceCommandId_t* id); + /** * @brief Each reply contains a status byte giving information about a request. This function * parses this byte and returns the associated failure message. @@ -149,12 +159,17 @@ private: 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. + * @brief This function handles all self test results. This comprises parsing the error byte + * and step byte and calling the function to fill the respective dataset. */ void handleSelfTestReply(const uint8_t* packet); + /** + * @brief The following functions fill the respective dataset of the single axis self tests. + * @param packet Pointer to the reply data holding the self test result. + */ + void handlePositiveXSelfTestReply(const uint8_t* packet); + /** * @brief This function checks the error byte of a self test measurement. * diff --git a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h index 0de6ab2f..e57f39d2 100644 --- a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h +++ b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h @@ -13,8 +13,13 @@ 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_CMD = 0x7; - static const DeviceCommandId_t GET_SELF_TEST_RESULT = 0x8; + static const DeviceCommandId_t POS_X_SELF_TEST = 0x7; + static const DeviceCommandId_t NEG_X_SELF_TEST = 0x8; + static const DeviceCommandId_t POS_Y_SELF_TEST = 0x9; + static const DeviceCommandId_t NEG_Y_SELF_TEST = 0x10; + static const DeviceCommandId_t POS_Z_SELF_TEST = 0x11; + static const DeviceCommandId_t NEG_Z_SELF_TEST = 0x12; + static const DeviceCommandId_t GET_SELF_TEST_RESULT = 0x13; static const uint8_t GET_TEMP_REPLY_SIZE = 2; static const uint8_t CFGR_CMD_SIZE = 3; @@ -48,6 +53,9 @@ namespace IMTQ { 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 + static const uint8_t INVALID_ERROR_BYTE = 0x80; // This is an invalid error byte and should be never replied by the IMTQ + + static const uint8_t MAIN_STEP_OFFSET = 43; /** * Command code definitions. Each command or reply of an IMTQ request will begin with one of @@ -107,19 +115,89 @@ namespace IMTQ { MTM_RAW_Z, 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, + INIT_POS_X_ERR, + INIT_POS_X_RAW_MAG_X, + INIT_POS_X_RAW_MAG_Y, + INIT_POS_X_RAW_MAG_Z, + INIT_POS_X_CAL_MAG_X, + INIT_POS_X_CAL_MAG_Y, + INIT_POS_X_CAL_MAG_Z, + INIT_POS_X_COIL_X_CURRENT, + INIT_POS_X_COIL_Y_CURRENT, + INIT_POS_X_COIL_Z_CURRENT, + INIT_POS_X_COIL_X_TEMPERATURE, + INIT_POS_X_COIL_Y_TEMPERATURE, + INIT_POS_X_COIL_Z_TEMPERATURE, + + INIT_NEG_X_ERR, + INIT_NEG_X_RAW_MAG_X, + INIT_NEG_X_RAW_MAG_Y, + INIT_NEG_X_RAW_MAG_Z, + INIT_NEG_X_CAL_MAG_X, + INIT_NEG_X_CAL_MAG_Y, + INIT_NEG_X_CAL_MAG_Z, + INIT_NEG_X_COIL_X_CURRENT, + INIT_NEG_X_COIL_Y_CURRENT, + INIT_NEG_X_COIL_Z_CURRENT, + INIT_NEG_X_COIL_X_TEMPERATURE, + INIT_NEG_X_COIL_Y_TEMPERATURE, + INIT_NEG_X_COIL_Z_TEMPERATURE, + + INIT_POS_Y_ERR, + INIT_POS_Y_RAW_MAG_X, + INIT_POS_Y_RAW_MAG_Y, + INIT_POS_Y_RAW_MAG_Z, + INIT_POS_Y_CAL_MAG_X, + INIT_POS_Y_CAL_MAG_Y, + INIT_POS_Y_CAL_MAG_Z, + INIT_POS_Y_COIL_X_CURRENT, + INIT_POS_Y_COIL_Y_CURRENT, + INIT_POS_Y_COIL_Z_CURRENT, + INIT_POS_Y_COIL_X_TEMPERATURE, + INIT_POS_Y_COIL_Y_TEMPERATURE, + INIT_POS_Y_COIL_Z_TEMPERATURE, + + INIT_NEG_Y_ERR, + INIT_NEG_Y_RAW_MAG_X, + INIT_NEG_Y_RAW_MAG_Y, + INIT_NEG_Y_RAW_MAG_Z, + INIT_NEG_Y_CAL_MAG_X, + INIT_NEG_Y_CAL_MAG_Y, + INIT_NEG_Y_CAL_MAG_Z, + INIT_NEG_Y_COIL_X_CURRENT, + INIT_NEG_Y_COIL_Y_CURRENT, + INIT_NEG_Y_COIL_Z_CURRENT, + INIT_NEG_Y_COIL_X_TEMPERATURE, + INIT_NEG_Y_COIL_Y_TEMPERATURE, + INIT_NEG_Y_COIL_Z_TEMPERATURE, + + INIT_POS_Z_ERR, + INIT_POS_Z_RAW_MAG_X, + INIT_POS_Z_RAW_MAG_Y, + INIT_POS_Z_RAW_MAG_Z, + INIT_POS_Z_CAL_MAG_X, + INIT_POS_Z_CAL_MAG_Y, + INIT_POS_Z_CAL_MAG_Z, + INIT_POS_Z_COIL_X_CURRENT, + INIT_POS_Z_COIL_Y_CURRENT, + INIT_POS_Z_COIL_Z_CURRENT, + INIT_POS_Z_COIL_X_TEMPERATURE, + INIT_POS_Z_COIL_Y_TEMPERATURE, + INIT_POS_Z_COIL_Z_TEMPERATURE, + + INIT_NEG_Z_ERR, + INIT_NEG_Z_RAW_MAG_X, + INIT_NEG_Z_RAW_MAG_Y, + INIT_NEG_Z_RAW_MAG_Z, + INIT_NEG_Z_CAL_MAG_X, + INIT_NEG_Z_CAL_MAG_Y, + INIT_NEG_Z_CAL_MAG_Z, + INIT_NEG_Z_COIL_X_CURRENT, + INIT_NEG_Z_COIL_Y_CURRENT, + INIT_NEG_Z_COIL_Z_CURRENT, + INIT_NEG_Z_COIL_X_TEMPERATURE, + INIT_NEG_Z_COIL_Y_TEMPERATURE, + INIT_NEG_Z_COIL_Z_TEMPERATURE, POS_X_ERR, POS_X_RAW_MAG_X, @@ -205,19 +283,89 @@ namespace IMTQ { 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, + FINA_POS_X_ERR, + FINA_POS_X_RAW_MAG_X, + FINA_POS_X_RAW_MAG_Y, + FINA_POS_X_RAW_MAG_Z, + FINA_POS_X_CAL_MAG_X, + FINA_POS_X_CAL_MAG_Y, + FINA_POS_X_CAL_MAG_Z, + FINA_POS_X_COIL_X_CURRENT, + FINA_POS_X_COIL_Y_CURRENT, + FINA_POS_X_COIL_Z_CURRENT, + FINA_POS_X_COIL_X_TEMPERATURE, + FINA_POS_X_COIL_Y_TEMPERATURE, + FINA_POS_X_COIL_Z_TEMPERATURE, + + FINA_NEG_X_ERR, + FINA_NEG_X_RAW_MAG_X, + FINA_NEG_X_RAW_MAG_Y, + FINA_NEG_X_RAW_MAG_Z, + FINA_NEG_X_CAL_MAG_X, + FINA_NEG_X_CAL_MAG_Y, + FINA_NEG_X_CAL_MAG_Z, + FINA_NEG_X_COIL_X_CURRENT, + FINA_NEG_X_COIL_Y_CURRENT, + FINA_NEG_X_COIL_Z_CURRENT, + FINA_NEG_X_COIL_X_TEMPERATURE, + FINA_NEG_X_COIL_Y_TEMPERATURE, + FINA_NEG_X_COIL_Z_TEMPERATURE, + + FINA_POS_Y_ERR, + FINA_POS_Y_RAW_MAG_X, + FINA_POS_Y_RAW_MAG_Y, + FINA_POS_Y_RAW_MAG_Z, + FINA_POS_Y_CAL_MAG_X, + FINA_POS_Y_CAL_MAG_Y, + FINA_POS_Y_CAL_MAG_Z, + FINA_POS_Y_COIL_X_CURRENT, + FINA_POS_Y_COIL_Y_CURRENT, + FINA_POS_Y_COIL_Z_CURRENT, + FINA_POS_Y_COIL_X_TEMPERATURE, + FINA_POS_Y_COIL_Y_TEMPERATURE, + FINA_POS_Y_COIL_Z_TEMPERATURE, + + FINA_NEG_Y_ERR, + FINA_NEG_Y_RAW_MAG_X, + FINA_NEG_Y_RAW_MAG_Y, + FINA_NEG_Y_RAW_MAG_Z, + FINA_NEG_Y_CAL_MAG_X, + FINA_NEG_Y_CAL_MAG_Y, + FINA_NEG_Y_CAL_MAG_Z, + FINA_NEG_Y_COIL_X_CURRENT, + FINA_NEG_Y_COIL_Y_CURRENT, + FINA_NEG_Y_COIL_Z_CURRENT, + FINA_NEG_Y_COIL_X_TEMPERATURE, + FINA_NEG_Y_COIL_Y_TEMPERATURE, + FINA_NEG_Y_COIL_Z_TEMPERATURE, + + FINA_POS_Z_ERR, + FINA_POS_Z_RAW_MAG_X, + FINA_POS_Z_RAW_MAG_Y, + FINA_POS_Z_RAW_MAG_Z, + FINA_POS_Z_CAL_MAG_X, + FINA_POS_Z_CAL_MAG_Y, + FINA_POS_Z_CAL_MAG_Z, + FINA_POS_Z_COIL_X_CURRENT, + FINA_POS_Z_COIL_Y_CURRENT, + FINA_POS_Z_COIL_Z_CURRENT, + FINA_POS_Z_COIL_X_TEMPERATURE, + FINA_POS_Z_COIL_Y_TEMPERATURE, + FINA_POS_Z_COIL_Z_TEMPERATURE, + + FINA_NEG_Z_ERR, + FINA_NEG_Z_RAW_MAG_X, + FINA_NEG_Z_RAW_MAG_Y, + FINA_NEG_Z_RAW_MAG_Z, + FINA_NEG_Z_CAL_MAG_X, + FINA_NEG_Z_CAL_MAG_Y, + FINA_NEG_Z_CAL_MAG_Z, + FINA_NEG_Z_COIL_X_CURRENT, + FINA_NEG_Z_COIL_Y_CURRENT, + FINA_NEG_Z_COIL_Z_CURRENT, + FINA_NEG_Z_COIL_X_TEMPERATURE, + FINA_NEG_Z_COIL_Y_TEMPERATURE, + FINA_NEG_Z_COIL_Z_TEMPERATURE, }; class EngHkDataset: public StaticLocalDataSet { @@ -296,7 +444,7 @@ public: }; /** - * @brief This dataset can be used to store the self test results. + * @brief This dataset can be used to store the self test results of the positve X axis test. * * @details Units of measurements: * Currents: [10^-4 A] @@ -306,193 +454,82 @@ public: * 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) + * 2. +X (apply current to generate dipole in positive X direction) + * 3. All coils off (FINA step) */ -class SelfTestDataset: public StaticLocalDataSet { +class PosXselfTestSet: public StaticLocalDataSet { public: - SelfTestDataset(HasLocalDataPoolIF* owner) : + PosXselfTestSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, SELF_TEST_DATA_SET) { } - SelfTestDataset(object_id_t objectId) : + PosXselfTestSet(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, + lp_var_t initErr = lp_var_t(sid.objectId, INIT_POS_X_ERR, this); + lp_var_t initRawMagX = lp_var_t(sid.objectId, INIT_POS_X_RAW_MAG_X, this); + lp_var_t initRawMagY = lp_var_t(sid.objectId, INIT_POS_X_RAW_MAG_Y, this); + lp_var_t initRawMagZ = lp_var_t(sid.objectId, INIT_POS_X_RAW_MAG_Z, this); + lp_var_t initCalMagX = lp_var_t(sid.objectId, INIT_POS_X_CAL_MAG_X, this); + lp_var_t initCalMagY = lp_var_t(sid.objectId, INIT_POS_X_CAL_MAG_Y, this); + lp_var_t initCalMagZ = lp_var_t(sid.objectId, INIT_POS_X_CAL_MAG_Z, this); + lp_var_t initCoilXCurrent = lp_var_t(sid.objectId, INIT_POS_X_COIL_X_CURRENT, this); - lp_var_t initCoilYCurrent = lp_var_t(sid.objectId, INIT_COIL_Y_CURRENT, + lp_var_t initCoilYCurrent = lp_var_t(sid.objectId, INIT_POS_X_COIL_Y_CURRENT, this); - lp_var_t initCoilZCurrent = lp_var_t(sid.objectId, INIT_COIL_Z_CURRENT, + lp_var_t initCoilZCurrent = lp_var_t(sid.objectId, INIT_POS_X_COIL_Z_CURRENT, this); lp_var_t initCoilXTemperature = lp_var_t(sid.objectId, - INIT_COIL_X_TEMPERATURE, this); + INIT_POS_X_COIL_X_TEMPERATURE, this); lp_var_t initCoilYTemperature = lp_var_t(sid.objectId, - INIT_COIL_Y_TEMPERATURE, this); + INIT_POS_X_COIL_Y_TEMPERATURE, this); lp_var_t initCoilZTemperature = lp_var_t(sid.objectId, - INIT_COIL_Z_TEMPERATURE, this); + INIT_POS_X_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, + lp_var_t err = lp_var_t(sid.objectId, POS_X_ERR, this); + lp_var_t rawMagX = lp_var_t(sid.objectId, POS_X_RAW_MAG_X, this); + lp_var_t rawMagY = lp_var_t(sid.objectId, POS_X_RAW_MAG_Y, this); + lp_var_t rawMagZ = lp_var_t(sid.objectId, POS_X_RAW_MAG_Z, this); + lp_var_t calMagX = lp_var_t(sid.objectId, POS_X_CAL_MAG_X, this); + lp_var_t calMagY = lp_var_t(sid.objectId, POS_X_CAL_MAG_Y, this); + lp_var_t calMagZ = lp_var_t(sid.objectId, POS_X_CAL_MAG_Z, this); + lp_var_t coilXCurrent = 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, + lp_var_t coilYCurrent = 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, + lp_var_t coilZCurrent = lp_var_t(sid.objectId, POS_X_COIL_Z_CURRENT, this); - lp_var_t posXCoilXTemperature = lp_var_t(sid.objectId, + lp_var_t coilXTemperature = lp_var_t(sid.objectId, POS_X_COIL_X_TEMPERATURE, this); - lp_var_t posXCoilYTemperature = lp_var_t(sid.objectId, + lp_var_t coilYTemperature = lp_var_t(sid.objectId, POS_X_COIL_Y_TEMPERATURE, this); - lp_var_t posXCoilZTemperature = lp_var_t(sid.objectId, + lp_var_t coilZTemperature = 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, + lp_var_t finaErr = lp_var_t(sid.objectId, FINA_POS_X_ERR, this); + lp_var_t finaRawMagX = lp_var_t(sid.objectId, FINA_POS_X_RAW_MAG_X, this); + lp_var_t finaRawMagY = lp_var_t(sid.objectId, FINA_POS_X_RAW_MAG_Y, this); + lp_var_t finaRawMagZ = lp_var_t(sid.objectId, FINA_POS_X_RAW_MAG_Z, this); + lp_var_t finaCalMagX = lp_var_t(sid.objectId, FINA_POS_X_CAL_MAG_X, this); + lp_var_t finaCalMagY = lp_var_t(sid.objectId, FINA_POS_X_CAL_MAG_Y, this); + lp_var_t finaCalMagZ = lp_var_t(sid.objectId, FINA_POS_X_CAL_MAG_Z, this); + lp_var_t finaCoilXCurrent = lp_var_t(sid.objectId, FINA_POS_X_COIL_X_CURRENT, this); - lp_var_t finaCoilYCurrent = lp_var_t(sid.objectId, FINA_COIL_Y_CURRENT, + lp_var_t finaCoilYCurrent = lp_var_t(sid.objectId, FINA_POS_X_COIL_Y_CURRENT, this); - lp_var_t finaCoilZCurrent = lp_var_t(sid.objectId, FINA_COIL_Z_CURRENT, + lp_var_t finaCoilZCurrent = lp_var_t(sid.objectId, FINA_POS_X_COIL_Z_CURRENT, this); lp_var_t finaCoilXTemperature = lp_var_t(sid.objectId, - FINA_COIL_X_TEMPERATURE, this); + FINA_POS_X_COIL_X_TEMPERATURE, this); lp_var_t finaCoilYTemperature = lp_var_t(sid.objectId, - FINA_COIL_Y_TEMPERATURE, this); + FINA_POS_X_COIL_Y_TEMPERATURE, this); lp_var_t finaCoilZTemperature = lp_var_t(sid.objectId, - FINA_COIL_Z_TEMPERATURE, this); - + FINA_POS_X_COIL_Z_TEMPERATURE, this); }; /**