From 5e7cdd9f7f92d0c6a3481acc1e43c760cc7fd5eb Mon Sep 17 00:00:00 2001 From: "Jakob.Meier" Date: Tue, 15 Jun 2021 15:13:47 +0200 Subject: [PATCH] IMTQ handler complete --- bsp_q7s/ObjectFactory.cpp | 3 +- fsfw | 2 +- mission/devices/IMTQHandler.cpp | 1599 +++++++++++++++-- mission/devices/IMTQHandler.h | 12 +- .../IMTQHandlerDefinitions.h | 549 +++++- 5 files changed, 1916 insertions(+), 249 deletions(-) diff --git a/bsp_q7s/ObjectFactory.cpp b/bsp_q7s/ObjectFactory.cpp index d9b82ea0..bc59b402 100644 --- a/bsp_q7s/ObjectFactory.cpp +++ b/bsp_q7s/ObjectFactory.cpp @@ -542,8 +542,7 @@ void ObjectFactory::produce(void* args){ I2cCookie* imtqI2cCookie = new I2cCookie(addresses::IMTQ, IMTQ::MAX_REPLY_SIZE, std::string("/dev/i2c-0")); - IMTQHandler* imtqHandler = new IMTQHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie); - imtqHandler->setStartUpImmediately(); + new IMTQHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie); UartCookie* plocUartCookie = new UartCookie(std::string("/dev/ttyUL3"), 115200, PLOC::MAX_REPLY_SIZE); diff --git a/fsfw b/fsfw index 16306825..d700fb55 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 1630682548a8775bd0c293b3c76c29e120de5bf0 +Subproject commit d700fb551c85393b58a3ada96fbd3f807217d14b diff --git a/mission/devices/IMTQHandler.cpp b/mission/devices/IMTQHandler.cpp index 35f0169f..bd5ed2d2 100644 --- a/mission/devices/IMTQHandler.cpp +++ b/mission/devices/IMTQHandler.cpp @@ -5,10 +5,11 @@ #include #include - IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) : DeviceHandlerBase(objectId, comIF, comCookie), engHkDataset(this), calMtmMeasurementSet( - this), rawMtmMeasurementSet(this), posXselfTestDataset(this) { + this), rawMtmMeasurementSet(this), posXselfTestDataset(this), negXselfTestDataset( + this), posYselfTestDataset(this), negYselfTestDataset(this), posZselfTestDataset( + this), negZselfTestDataset(this) { if (comCookie == NULL) { sif::error << "IMTQHandler: Invalid com cookie" << std::endl; } @@ -17,21 +18,19 @@ IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF * com IMTQHandler::~IMTQHandler() { } - -void IMTQHandler::doStartUp(){ +void IMTQHandler::doStartUp() { #if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1 - setMode(MODE_NORMAL); + setMode(MODE_NORMAL); #else - setMode(_MODE_TO_ON); + setMode(_MODE_TO_ON); #endif } -void IMTQHandler::doShutDown(){ +void IMTQHandler::doShutDown() { } -ReturnValue_t IMTQHandler::buildNormalDeviceCommand( - DeviceCommandId_t * id) { +ReturnValue_t IMTQHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) { switch (communicationStep) { case CommunicationStep::GET_ENG_HK_DATA: *id = IMTQ::GET_ENG_HK_DATA; @@ -51,35 +50,68 @@ ReturnValue_t IMTQHandler::buildNormalDeviceCommand( break; default: sif::debug << "IMTQHandler::buildNormalDeviceCommand: Invalid communication step" - << std::endl; + << std::endl; break; } return buildCommandFromCommand(*id, NULL, 0); } -ReturnValue_t IMTQHandler::buildTransitionDeviceCommand( - DeviceCommandId_t * id){ - return RETURN_OK; +ReturnValue_t IMTQHandler::buildTransitionDeviceCommand(DeviceCommandId_t * id) { + return RETURN_OK; } -ReturnValue_t IMTQHandler::buildCommandFromCommand( - DeviceCommandId_t deviceCommand, const uint8_t * commandData, - size_t commandDataLen) { - switch(deviceCommand) { - case(IMTQ::POS_X_SELF_TEST): { - commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; - commandBuffer[1] = IMTQ::SELF_TEST_AXIS::X_POSITIVE; - rawPacket = commandBuffer; +ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t * commandData, size_t commandDataLen) { + switch (deviceCommand) { + case (IMTQ::POS_X_SELF_TEST): { + commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; + commandBuffer[1] = IMTQ::SELF_TEST_AXIS::X_POSITIVE; + rawPacket = commandBuffer; rawPacketLen = 2; - return RETURN_OK; - } - case(IMTQ::GET_SELF_TEST_RESULT): { - commandBuffer[0] = IMTQ::CC::GET_SELF_TEST_RESULT; - rawPacket = commandBuffer; + return RETURN_OK; + } + case (IMTQ::NEG_X_SELF_TEST): { + commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; + commandBuffer[1] = IMTQ::SELF_TEST_AXIS::X_NEGATIVE; + rawPacket = commandBuffer; + rawPacketLen = 2; + return RETURN_OK; + } + case (IMTQ::POS_Y_SELF_TEST): { + commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; + commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Y_POSITIVE; + rawPacket = commandBuffer; + rawPacketLen = 2; + return RETURN_OK; + } + case (IMTQ::NEG_Y_SELF_TEST): { + commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; + commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Y_NEGATIVE; + rawPacket = commandBuffer; + rawPacketLen = 2; + return RETURN_OK; + } + case (IMTQ::POS_Z_SELF_TEST): { + commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; + commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Z_POSITIVE; + rawPacket = commandBuffer; + rawPacketLen = 2; + return RETURN_OK; + } + case (IMTQ::NEG_Z_SELF_TEST): { + commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; + commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Z_NEGATIVE; + rawPacket = commandBuffer; + rawPacketLen = 2; + return 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): { + return RETURN_OK; + } + case (IMTQ::START_ACTUATION_DIPOLE): { /* IMTQ expects low byte first */ commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE; commandBuffer[1] = *(commandData + 1); @@ -94,55 +126,49 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand( rawPacketLen = 9; return RETURN_OK; } - case(IMTQ::GET_ENG_HK_DATA): { - commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA; - rawPacket = commandBuffer; - rawPacketLen = 1; - return RETURN_OK; - } - case(IMTQ::GET_COMMANDED_DIPOLE): { - commandBuffer[0] = IMTQ::CC::GET_COMMANDED_DIPOLE; - rawPacket = commandBuffer; - rawPacketLen = 1; - return RETURN_OK; - } - case(IMTQ::START_MTM_MEASUREMENT): { - commandBuffer[0] = IMTQ::CC::START_MTM_MEASUREMENT; - rawPacket = commandBuffer; - rawPacketLen = 1; - return RETURN_OK; - } - case(IMTQ::GET_CAL_MTM_MEASUREMENT): { - commandBuffer[0] = IMTQ::CC::GET_CAL_MTM_MEASUREMENT; - rawPacket = commandBuffer; - rawPacketLen = 1; - return RETURN_OK; - } - case(IMTQ::GET_RAW_MTM_MEASUREMENT): { - commandBuffer[0] = IMTQ::CC::GET_RAW_MTM_MEASUREMENT; - rawPacket = commandBuffer; - rawPacketLen = 1; - return RETURN_OK; - } - default: - return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; - } - return HasReturnvaluesIF::RETURN_FAILED; + case (IMTQ::GET_ENG_HK_DATA): { + commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA; + rawPacket = commandBuffer; + rawPacketLen = 1; + return RETURN_OK; + } + case (IMTQ::GET_COMMANDED_DIPOLE): { + commandBuffer[0] = IMTQ::CC::GET_COMMANDED_DIPOLE; + rawPacket = commandBuffer; + rawPacketLen = 1; + return RETURN_OK; + } + case (IMTQ::START_MTM_MEASUREMENT): { + commandBuffer[0] = IMTQ::CC::START_MTM_MEASUREMENT; + rawPacket = commandBuffer; + rawPacketLen = 1; + return RETURN_OK; + } + case (IMTQ::GET_CAL_MTM_MEASUREMENT): { + commandBuffer[0] = IMTQ::CC::GET_CAL_MTM_MEASUREMENT; + rawPacket = commandBuffer; + rawPacketLen = 1; + return RETURN_OK; + } + case (IMTQ::GET_RAW_MTM_MEASUREMENT): { + commandBuffer[0] = IMTQ::CC::GET_RAW_MTM_MEASUREMENT; + rawPacket = commandBuffer; + rawPacketLen = 1; + return RETURN_OK; + } + default: + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } + return HasReturnvaluesIF::RETURN_FAILED; } void IMTQHandler::fillCommandAndReplyMap() { - this->insertInCommandAndReplyMap(IMTQ::POS_X_SELF_TEST, 1, nullptr, - IMTQ::SIZE_STATUS_REPLY); - this->insertInCommandAndReplyMap(IMTQ::NEG_X_SELF_TEST, 1, nullptr, - IMTQ::SIZE_STATUS_REPLY); - this->insertInCommandAndReplyMap(IMTQ::POS_Y_SELF_TEST, 1, nullptr, - IMTQ::SIZE_STATUS_REPLY); - this->insertInCommandAndReplyMap(IMTQ::NEG_Y_SELF_TEST, 1, nullptr, - IMTQ::SIZE_STATUS_REPLY); - this->insertInCommandAndReplyMap(IMTQ::POS_Z_SELF_TEST, 1, nullptr, - IMTQ::SIZE_STATUS_REPLY); - this->insertInCommandAndReplyMap(IMTQ::NEG_Z_SELF_TEST, 1, nullptr, - IMTQ::SIZE_STATUS_REPLY); + this->insertInCommandAndReplyMap(IMTQ::POS_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + this->insertInCommandAndReplyMap(IMTQ::NEG_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + this->insertInCommandAndReplyMap(IMTQ::POS_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + this->insertInCommandAndReplyMap(IMTQ::NEG_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + this->insertInCommandAndReplyMap(IMTQ::POS_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + this->insertInCommandAndReplyMap(IMTQ::NEG_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); this->insertInCommandAndReplyMap(IMTQ::GET_SELF_TEST_RESULT, 1, nullptr, IMTQ::SIZE_SELF_TEST_RESULTS); this->insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr, @@ -159,55 +185,54 @@ void IMTQHandler::fillCommandAndReplyMap() { IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT); } -ReturnValue_t IMTQHandler::scanForReply(const uint8_t *start, - size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) { +ReturnValue_t IMTQHandler::scanForReply(const uint8_t *start, size_t remainingSize, + DeviceCommandId_t *foundId, size_t *foundLen) { ReturnValue_t result = RETURN_OK; - switch(*start) { - case(IMTQ::CC::START_ACTUATION_DIPOLE): + switch (*start) { + case (IMTQ::CC::START_ACTUATION_DIPOLE): *foundLen = IMTQ::SIZE_STATUS_REPLY; *foundId = IMTQ::START_ACTUATION_DIPOLE; - break; - case(IMTQ::CC::START_MTM_MEASUREMENT): + break; + case (IMTQ::CC::START_MTM_MEASUREMENT): *foundLen = IMTQ::SIZE_STATUS_REPLY; *foundId = IMTQ::START_MTM_MEASUREMENT; - break; - case(IMTQ::CC::GET_ENG_HK_DATA): + break; + case (IMTQ::CC::GET_ENG_HK_DATA): *foundLen = IMTQ::SIZE_ENG_HK_DATA_REPLY; *foundId = IMTQ::GET_ENG_HK_DATA; - break; - case(IMTQ::CC::GET_COMMANDED_DIPOLE): + break; + case (IMTQ::CC::GET_COMMANDED_DIPOLE): *foundLen = IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY; *foundId = IMTQ::GET_COMMANDED_DIPOLE; - break; - case(IMTQ::CC::GET_CAL_MTM_MEASUREMENT): + break; + case (IMTQ::CC::GET_CAL_MTM_MEASUREMENT): *foundLen = IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT; *foundId = IMTQ::GET_CAL_MTM_MEASUREMENT; - break; - case(IMTQ::CC::GET_RAW_MTM_MEASUREMENT): + break; + case (IMTQ::CC::GET_RAW_MTM_MEASUREMENT): *foundLen = IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT; *foundId = IMTQ::GET_RAW_MTM_MEASUREMENT; - break; - case(IMTQ::CC::SELF_TEST_CMD): + break; + case (IMTQ::CC::SELF_TEST_CMD): *foundLen = IMTQ::SIZE_STATUS_REPLY; - result = getSelfTestCommandId(foundId); - break; - case(IMTQ::CC::GET_SELF_TEST_RESULT): + result = getSelfTestCommandId(foundId); + break; + case (IMTQ::CC::GET_SELF_TEST_RESULT): *foundLen = IMTQ::SIZE_SELF_TEST_RESULTS; *foundId = IMTQ::GET_SELF_TEST_RESULT; - break; - default: + break; + default: sif::debug << "IMTQHandler::scanForReply: Reply contains invalid command code" << std::endl; result = IGNORE_REPLY_DATA; - break; - } + break; + } return result; } -ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, - const uint8_t *packet) { +ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { ReturnValue_t result = RETURN_OK; @@ -245,37 +270,43 @@ ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, break; default: { sif::debug << "IMTQHandler::interpretDeviceReply: Unknown device reply id" << std::endl; - return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; - } - } + return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; + } + } - return RETURN_OK; + return RETURN_OK; } -void IMTQHandler::setNormalDatapoolEntriesInvalid(){ +void IMTQHandler::setNormalDatapoolEntriesInvalid() { } LocalPoolDataSetBase* IMTQHandler::getDataSetHandle(sid_t sid) { if (sid == engHkDataset.getSid()) { return &engHkDataset; - } - else if (sid== calMtmMeasurementSet.getSid()) { + } else if (sid == calMtmMeasurementSet.getSid()) { return &calMtmMeasurementSet; - } - else if (sid== rawMtmMeasurementSet.getSid()) { + } else if (sid == rawMtmMeasurementSet.getSid()) { return &rawMtmMeasurementSet; - } - else if (sid== posXselfTestDataset.getSid()) { + } else if (sid == posXselfTestDataset.getSid()) { return &posXselfTestDataset; - } - else { + } else if (sid == negXselfTestDataset.getSid()) { + return &negXselfTestDataset; + } else if (sid == posYselfTestDataset.getSid()) { + return &posYselfTestDataset; + } else if (sid == negYselfTestDataset.getSid()) { + return &negYselfTestDataset; + } else if (sid == posZselfTestDataset.getSid()) { + return &posZselfTestDataset; + } else if (sid == negZselfTestDataset.getSid()) { + return &negZselfTestDataset; + } else { sif::error << "IMTQHandler::getDataSetHandle: Invalid sid" << std::endl; return nullptr; } } -uint32_t IMTQHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo){ +uint32_t IMTQHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; } @@ -307,7 +338,7 @@ 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 of positive X axis test */ + /** INIT measurements for positive X axis test */ localDataPoolMap.emplace(IMTQ::INIT_POS_X_ERR, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::INIT_POS_X_RAW_MAG_X, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::INIT_POS_X_RAW_MAG_Y, new PoolEntry( { 0 })); @@ -322,6 +353,81 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + /** INIT measurements for negative X axis test */ + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + + /** INIT measurements for positive Y axis test */ + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + + /** INIT measurements for negative Y axis test */ + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + + /** INIT measurements for positive Z axis test */ + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + + /** INIT measurements for negative Z axis test */ + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::POS_X_ERR, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_X_RAW_MAG_X, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_X_RAW_MAG_Y, new PoolEntry( { 0 })); @@ -336,20 +442,6 @@ 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::FINA_POS_X_ERR, new PoolEntry( { 0 })); - localDataPoolMap.emplace(IMTQ::FINA_POS_X_RAW_MAG_X, new PoolEntry( { 0 })); - localDataPoolMap.emplace(IMTQ::FINA_POS_X_RAW_MAG_Y, new PoolEntry( { 0 })); - localDataPoolMap.emplace(IMTQ::FINA_POS_X_RAW_MAG_Z, new PoolEntry( { 0 })); - localDataPoolMap.emplace(IMTQ::FINA_POS_X_CAL_MAG_X, new PoolEntry( { 0 })); - localDataPoolMap.emplace(IMTQ::FINA_POS_X_CAL_MAG_Y, new PoolEntry( { 0 })); - localDataPoolMap.emplace(IMTQ::FINA_POS_X_CAL_MAG_Z, new PoolEntry( { 0 })); - localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_X_CURRENT, new PoolEntry( { 0 })); - localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Y_CURRENT, new PoolEntry( { 0 })); - localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Z_CURRENT, new PoolEntry( { 0 })); - localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); - localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); - localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); - 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 })); @@ -420,31 +512,121 @@ 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 })); + /** FINA measurements for positive X axis test */ + localDataPoolMap.emplace(IMTQ::FINA_POS_X_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + + /** FINA measurements for negative X axis test */ + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + + /** FINA measurements for positive Y axis test */ + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + + /** FINA measurements for negative Y axis test */ + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + + /** FINA measurements for positive Z axis test */ + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + + /** FINA measurements for negative Z axis test */ + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + 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 " + 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 UNEXPECTED_SELF_TEST_REPLY; } return RETURN_OK; } ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) { uint8_t cmdErrorField = *(packet + 1) & 0xF; - switch (cmdErrorField) { + switch (cmdErrorField) { case 0: return RETURN_OK; case 1: @@ -468,7 +650,7 @@ ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) { return INTERNAL_PROCESSING_ERROR; default: sif::error << "IMTQHandler::parseStatusByte: CMD Error field contains unknown error code " - << cmdErrorField << std::endl; + << cmdErrorField << std::endl; return CMD_ERR_UNKNOWN; } } @@ -484,11 +666,14 @@ void IMTQHandler::fillEngHkDataset(const uint8_t* packet) { offset += 2; engHkDataset.analogCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; - engHkDataset.coilXCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; + engHkDataset.coilXCurrentmA = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; offset += 2; - engHkDataset.coilYCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; + engHkDataset.coilYCurrentmA = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; offset += 2; - engHkDataset.coilZCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; + engHkDataset.coilZCurrentmA = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; offset += 2; engHkDataset.coilXTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); offset += 2; @@ -614,11 +799,31 @@ void IMTQHandler::handleSelfTestReply(const uint8_t* packet) { uint16_t offset = 2; checkErrorByte(*(packet + offset), *(packet + offset + 1)); - switch(*(packet + IMTQ::MAIN_STEP_OFFSET)) { + switch (*(packet + IMTQ::MAIN_STEP_OFFSET)) { case IMTQ::SELF_TEST_STEPS::X_POSITIVE: { handlePositiveXSelfTestReply(packet); break; } + case IMTQ::SELF_TEST_STEPS::X_NEGATIVE: { + handleNegativeXSelfTestReply(packet); + break; + } + case IMTQ::SELF_TEST_STEPS::Y_POSITIVE: { + handlePositiveYSelfTestReply(packet); + break; + } + case IMTQ::SELF_TEST_STEPS::Y_NEGATIVE: { + handleNegativeYSelfTestReply(packet); + break; + } + case IMTQ::SELF_TEST_STEPS::Z_POSITIVE: { + handlePositiveZSelfTestReply(packet); + break; + } + case IMTQ::SELF_TEST_STEPS::Z_NEGATIVE: { + handleNegativeZSelfTestReply(packet); + break; + } default: break; } @@ -649,14 +854,14 @@ void IMTQHandler::handlePositiveXSelfTestReply(const uint8_t* packet) { posXselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; - posXselfTestDataset.initCoilXCurrent = (*(packet + offset + 1) << 8 | *(packet + offset)) - * 0.1; + posXselfTestDataset.initCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; offset += 2; - posXselfTestDataset.initCoilYCurrent = (*(packet + offset + 1) << 8 | *(packet + offset)) - * 0.1; + posXselfTestDataset.initCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; offset += 2; - posXselfTestDataset.initCoilZCurrent = (*(packet + offset + 1) << 8 | *(packet + offset)) - * 0.1; + posXselfTestDataset.initCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; offset += 2; posXselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; @@ -687,14 +892,14 @@ void IMTQHandler::handlePositiveXSelfTestReply(const uint8_t* packet) { posXselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; - posXselfTestDataset.coilXCurrent = (*(packet + offset + 1) << 8 | *(packet + offset)) - * 0.1; + posXselfTestDataset.coilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; offset += 2; - posXselfTestDataset.coilYCurrent = (*(packet + offset + 1) << 8 | *(packet + offset)) - * 0.1; + posXselfTestDataset.coilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; offset += 2; - posXselfTestDataset.coilZCurrent = (*(packet + offset + 1) << 8 | *(packet + offset)) - * 0.1; + posXselfTestDataset.coilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; offset += 2; posXselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; @@ -725,14 +930,14 @@ void IMTQHandler::handlePositiveXSelfTestReply(const uint8_t* packet) { posXselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; - posXselfTestDataset.finaCoilXCurrent = (*(packet + offset + 1) << 8 | *(packet + offset)) - * 0.1; + posXselfTestDataset.finaCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; offset += 2; - posXselfTestDataset.finaCoilYCurrent = (*(packet + offset + 1) << 8 | *(packet + offset)) - * 0.1; + posXselfTestDataset.finaCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; offset += 2; - posXselfTestDataset.finaCoilZCurrent = (*(packet + offset + 1) << 8 | *(packet + offset)) - * 0.1; + posXselfTestDataset.finaCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; offset += 2; posXselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; @@ -743,19 +948,19 @@ void IMTQHandler::handlePositiveXSelfTestReply(const uint8_t* packet) { #if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1 sif::info << "IMTQ self test (INIT) err: " - << static_cast(posXselfTestDataset.initErr.value) << std::endl; + << static_cast(posXselfTestDataset.initErr.value) << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field X: " << posXselfTestDataset.initRawMagX << " nT" << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << posXselfTestDataset.initRawMagY << " nT" << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << posXselfTestDataset.initRawMagZ << " nT" << std::endl; - sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " << posXselfTestDataset.initCalMagX - << " nT" << std::endl; - sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " << posXselfTestDataset.initCalMagY - << " nT" << std::endl; - sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " << posXselfTestDataset.initCalMagZ - << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " + << posXselfTestDataset.initCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " + << posXselfTestDataset.initCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " + << posXselfTestDataset.initCalMagZ << " nT" << std::endl; sif::info << "IMTQ self test (INIT) coil X current: " << posXselfTestDataset.initCoilXCurrent << " mA" << std::endl; sif::info << "IMTQ self test (INIT) coil Y current: " << posXselfTestDataset.initCoilYCurrent @@ -769,8 +974,8 @@ void IMTQHandler::handlePositiveXSelfTestReply(const uint8_t* packet) { sif::info << "IMTQ self test (INIT) coil Z temperature: " << posXselfTestDataset.initCoilZTemperature << " °C" << std::endl; - sif::info << "IMTQ self test (+X) err: " << - static_cast(posXselfTestDataset.err.value) << std::endl; + sif::info << "IMTQ self test (+X) err: " + << static_cast(posXselfTestDataset.err.value) << std::endl; sif::info << "IMTQ self test (+X) raw magnetic field X: " << posXselfTestDataset.rawMagX << " nT" << std::endl; sif::info << "IMTQ self test (+X) raw magnetic field Y: " << posXselfTestDataset.rawMagY @@ -783,18 +988,18 @@ void IMTQHandler::handlePositiveXSelfTestReply(const uint8_t* packet) { << " nT" << std::endl; sif::info << "IMTQ self test (+X) calibrated magnetic field Z: " << posXselfTestDataset.calMagZ << " nT" << std::endl; - sif::info << "IMTQ self test (+X) coil X current: " << posXselfTestDataset.coilXCurrent - << " mA" << std::endl; - sif::info << "IMTQ self test (+X) coil Y current: " << posXselfTestDataset.coilYCurrent - << " mA" << std::endl; - sif::info << "IMTQ self test (+X) coil Z current: " << posXselfTestDataset.coilZCurrent - << " mA" << std::endl; - sif::info << "IMTQ self test (+X) coil X temperature: " - << posXselfTestDataset.coilXTemperature << " °C" << std::endl; - sif::info << "IMTQ self test (+X) coil Y temperature: " - << posXselfTestDataset.coilYTemperature << " °C" << std::endl; - sif::info << "IMTQ self test (+X) coil Z temperature: " - << posXselfTestDataset.coilZTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (+X) coil X current: " << posXselfTestDataset.coilXCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (+X) coil Y current: " << posXselfTestDataset.coilYCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (+X) coil Z current: " << posXselfTestDataset.coilZCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (+X) coil X temperature: " << posXselfTestDataset.coilXTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (+X) coil Y temperature: " << posXselfTestDataset.coilYTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (+X) coil Z temperature: " << posXselfTestDataset.coilZTemperature + << " °C" << std::endl; sif::info << "IMTQ self test (FINA) err: " << static_cast(posXselfTestDataset.finaErr.value) << std::endl; @@ -804,12 +1009,12 @@ void IMTQHandler::handlePositiveXSelfTestReply(const uint8_t* packet) { << " nT" << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << posXselfTestDataset.finaRawMagZ << " nT" << std::endl; - sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " << posXselfTestDataset.finaCalMagX - << " nT" << std::endl; - sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " << posXselfTestDataset.finaCalMagY - << " nT" << std::endl; - sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " << posXselfTestDataset.finaCalMagZ - << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " + << posXselfTestDataset.finaCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " + << posXselfTestDataset.finaCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " + << posXselfTestDataset.finaCalMagZ << " nT" << std::endl; sif::info << "IMTQ self test (FINA) coil X current: " << posXselfTestDataset.finaCoilXCurrent << " mA" << std::endl; sif::info << "IMTQ self test (FINA) coil Y current: " << posXselfTestDataset.finaCoilYCurrent @@ -825,12 +1030,1016 @@ void IMTQHandler::handlePositiveXSelfTestReply(const uint8_t* packet) { #endif } +void IMTQHandler::handleNegativeXSelfTestReply(const uint8_t* packet) { + PoolReadGuard rg(&posXselfTestDataset); + + uint16_t offset = 2; + /** Init measurements */ + negXselfTestDataset.initErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + negXselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negXselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negXselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negXselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negXselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negXselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negXselfTestDataset.initCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negXselfTestDataset.initCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negXselfTestDataset.initCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negXselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negXselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negXselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** +X measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + negXselfTestDataset.err = *(packet + offset); + offset += 2; // STEP byte will not be stored + negXselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negXselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negXselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negXselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negXselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negXselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negXselfTestDataset.coilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negXselfTestDataset.coilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negXselfTestDataset.coilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negXselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negXselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negXselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** FINA measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + negXselfTestDataset.finaErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + negXselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negXselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negXselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negXselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negXselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negXselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negXselfTestDataset.finaCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negXselfTestDataset.finaCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negXselfTestDataset.finaCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negXselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negXselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negXselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + +#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1 + sif::info << "IMTQ self test (INIT) err: " + << static_cast(negXselfTestDataset.initErr.value) << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field X: " << negXselfTestDataset.initRawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << negXselfTestDataset.initRawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << negXselfTestDataset.initRawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " + << negXselfTestDataset.initCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " + << negXselfTestDataset.initCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " + << negXselfTestDataset.initCalMagZ << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) coil X current: " << negXselfTestDataset.initCoilXCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y current: " << negXselfTestDataset.initCoilYCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z current: " << negXselfTestDataset.initCoilZCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil X temperature: " + << negXselfTestDataset.initCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y temperature: " + << negXselfTestDataset.initCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z temperature: " + << negXselfTestDataset.initCoilZTemperature << " °C" << std::endl; + + sif::info << "IMTQ self test (-X) err: " + << static_cast(negXselfTestDataset.err.value) << std::endl; + sif::info << "IMTQ self test (-X) raw magnetic field X: " << negXselfTestDataset.rawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (-X) raw magnetic field Y: " << negXselfTestDataset.rawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (-X) raw magnetic field Z: " << negXselfTestDataset.rawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (-X) calibrated magnetic field X: " << negXselfTestDataset.calMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (-X) calibrated magnetic field Y: " << negXselfTestDataset.calMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (-X) calibrated magnetic field Z: " << negXselfTestDataset.calMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (-X) coil X current: " << negXselfTestDataset.coilXCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (-X) coil Y current: " << negXselfTestDataset.coilYCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (-X) coil Z current: " << negXselfTestDataset.coilZCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (-X) coil X temperature: " << negXselfTestDataset.coilXTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (-X) coil Y temperature: " << negXselfTestDataset.coilYTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (-X) coil Z temperature: " << negXselfTestDataset.coilZTemperature + << " °C" << std::endl; + + sif::info << "IMTQ self test (FINA) err: " + << static_cast(negXselfTestDataset.finaErr.value) << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field X: " << negXselfTestDataset.finaRawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << negXselfTestDataset.finaRawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << negXselfTestDataset.finaRawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " + << negXselfTestDataset.finaCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " + << negXselfTestDataset.finaCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " + << negXselfTestDataset.finaCalMagZ << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) coil X current: " << negXselfTestDataset.finaCoilXCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y current: " << negXselfTestDataset.finaCoilYCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z current: " << negXselfTestDataset.finaCoilZCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil X temperature: " + << negXselfTestDataset.finaCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y temperature: " + << negXselfTestDataset.finaCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z temperature: " + << negXselfTestDataset.finaCoilZTemperature << " °C" << std::endl; +#endif +} + +void IMTQHandler::handlePositiveYSelfTestReply(const uint8_t* packet) { + PoolReadGuard rg(&posXselfTestDataset); + + uint16_t offset = 2; + /** Init measurements */ + posYselfTestDataset.initErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + posYselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posYselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posYselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posYselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posYselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posYselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posYselfTestDataset.initCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posYselfTestDataset.initCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posYselfTestDataset.initCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posYselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posYselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posYselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** +X measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + posYselfTestDataset.err = *(packet + offset); + offset += 2; // STEP byte will not be stored + posYselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posYselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posYselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posYselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posYselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posYselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posYselfTestDataset.coilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posYselfTestDataset.coilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posYselfTestDataset.coilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posYselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posYselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posYselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** FINA measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + posYselfTestDataset.finaErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + posYselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posYselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posYselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posYselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posYselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posYselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posYselfTestDataset.finaCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posYselfTestDataset.finaCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posYselfTestDataset.finaCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posYselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posYselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posYselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + +#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1 + sif::info << "IMTQ self test (INIT) err: " + << static_cast(posYselfTestDataset.initErr.value) << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field X: " << posYselfTestDataset.initRawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << posYselfTestDataset.initRawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << posYselfTestDataset.initRawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " + << posYselfTestDataset.initCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " + << posYselfTestDataset.initCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " + << posYselfTestDataset.initCalMagZ << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) coil X current: " << posYselfTestDataset.initCoilXCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y current: " << posYselfTestDataset.initCoilYCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z current: " << posYselfTestDataset.initCoilZCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil X temperature: " + << posYselfTestDataset.initCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y temperature: " + << posYselfTestDataset.initCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z temperature: " + << posYselfTestDataset.initCoilZTemperature << " °C" << std::endl; + + sif::info << "IMTQ self test (+Y) err: " + << static_cast(posYselfTestDataset.err.value) << std::endl; + sif::info << "IMTQ self test (+Y) raw magnetic field X: " << posYselfTestDataset.rawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (+Y) raw magnetic field Y: " << posYselfTestDataset.rawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (+Y) raw magnetic field Z: " << posYselfTestDataset.rawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (+Y) calibrated magnetic field X: " << posYselfTestDataset.calMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (+Y) calibrated magnetic field Y: " << posYselfTestDataset.calMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (+Y) calibrated magnetic field Z: " << posYselfTestDataset.calMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (+Y) coil X current: " << posYselfTestDataset.coilXCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (+Y) coil Y current: " << posYselfTestDataset.coilYCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (+Y) coil Z current: " << posYselfTestDataset.coilZCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (+Y) coil X temperature: " << posYselfTestDataset.coilXTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (+Y) coil Y temperature: " << posYselfTestDataset.coilYTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (+Y) coil Z temperature: " << posYselfTestDataset.coilZTemperature + << " °C" << std::endl; + + sif::info << "IMTQ self test (FINA) err: " + << static_cast(posYselfTestDataset.finaErr.value) << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field X: " << posYselfTestDataset.finaRawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << posYselfTestDataset.finaRawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << posYselfTestDataset.finaRawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " + << posYselfTestDataset.finaCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " + << posYselfTestDataset.finaCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " + << posYselfTestDataset.finaCalMagZ << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) coil X current: " << posYselfTestDataset.finaCoilXCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y current: " << posYselfTestDataset.finaCoilYCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z current: " << posYselfTestDataset.finaCoilZCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil X temperature: " + << posYselfTestDataset.finaCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y temperature: " + << posYselfTestDataset.finaCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z temperature: " + << posYselfTestDataset.finaCoilZTemperature << " °C" << std::endl; +#endif +} + +void IMTQHandler::handleNegativeYSelfTestReply(const uint8_t* packet) { + PoolReadGuard rg(&posXselfTestDataset); + + uint16_t offset = 2; + /** Init measurements */ + posZselfTestDataset.initErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + negYselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negYselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negYselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negYselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negYselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negYselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negYselfTestDataset.initCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negYselfTestDataset.initCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negYselfTestDataset.initCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negYselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negYselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negYselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** +X measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + negYselfTestDataset.err = *(packet + offset); + offset += 2; // STEP byte will not be stored + negYselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negYselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negYselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negYselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negYselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negYselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negYselfTestDataset.coilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negYselfTestDataset.coilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negYselfTestDataset.coilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negYselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negYselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negYselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** FINA measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + negYselfTestDataset.finaErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + negYselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negYselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negYselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negYselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negYselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negYselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negYselfTestDataset.finaCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negYselfTestDataset.finaCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negYselfTestDataset.finaCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negYselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negYselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negYselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + +#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1 + sif::info << "IMTQ self test (INIT) err: " + << static_cast(negYselfTestDataset.initErr.value) << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field X: " << negYselfTestDataset.initRawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << negYselfTestDataset.initRawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << negYselfTestDataset.initRawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " + << negYselfTestDataset.initCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " + << negYselfTestDataset.initCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " + << negYselfTestDataset.initCalMagZ << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) coil X current: " << negYselfTestDataset.initCoilXCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y current: " << negYselfTestDataset.initCoilYCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z current: " << negYselfTestDataset.initCoilZCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil X temperature: " + << negYselfTestDataset.initCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y temperature: " + << negYselfTestDataset.initCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z temperature: " + << negYselfTestDataset.initCoilZTemperature << " °C" << std::endl; + + sif::info << "IMTQ self test (-Y) err: " + << static_cast(negYselfTestDataset.err.value) << std::endl; + sif::info << "IMTQ self test (-Y) raw magnetic field X: " << negYselfTestDataset.rawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (-Y) raw magnetic field Y: " << negYselfTestDataset.rawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (-Y) raw magnetic field Z: " << negYselfTestDataset.rawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (-Y) calibrated magnetic field X: " << negYselfTestDataset.calMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (-Y) calibrated magnetic field Y: " << negYselfTestDataset.calMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (-Y) calibrated magnetic field Z: " << negYselfTestDataset.calMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (-Y) coil X current: " << negYselfTestDataset.coilXCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (-Y) coil Y current: " << negYselfTestDataset.coilYCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (-Y) coil Z current: " << negYselfTestDataset.coilZCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (-Y) coil X temperature: " << negYselfTestDataset.coilXTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (-Y) coil Y temperature: " << negYselfTestDataset.coilYTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (-Y) coil Z temperature: " << negYselfTestDataset.coilZTemperature + << " °C" << std::endl; + + sif::info << "IMTQ self test (FINA) err: " + << static_cast(negYselfTestDataset.finaErr.value) << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field X: " << negYselfTestDataset.finaRawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << negYselfTestDataset.finaRawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << negYselfTestDataset.finaRawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " + << negYselfTestDataset.finaCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " + << negYselfTestDataset.finaCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " + << negYselfTestDataset.finaCalMagZ << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) coil X current: " << negYselfTestDataset.finaCoilXCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y current: " << negYselfTestDataset.finaCoilYCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z current: " << negYselfTestDataset.finaCoilZCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil X temperature: " + << negYselfTestDataset.finaCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y temperature: " + << negYselfTestDataset.finaCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z temperature: " + << negYselfTestDataset.finaCoilZTemperature << " °C" << std::endl; +#endif +} + +void IMTQHandler::handlePositiveZSelfTestReply(const uint8_t* packet) { + PoolReadGuard rg(&posXselfTestDataset); + + uint16_t offset = 2; + /** Init measurements */ + posZselfTestDataset.initErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + posZselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posZselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posZselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posZselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posZselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posZselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posZselfTestDataset.initCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posZselfTestDataset.initCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posZselfTestDataset.initCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posZselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posZselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posZselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** +X measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + posZselfTestDataset.err = *(packet + offset); + offset += 2; // STEP byte will not be stored + posZselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posZselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posZselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posZselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posZselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posZselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posZselfTestDataset.coilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posZselfTestDataset.coilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posZselfTestDataset.coilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posZselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posZselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posZselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** FINA measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + posZselfTestDataset.finaErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + posZselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posZselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posZselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posZselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posZselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posZselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posZselfTestDataset.finaCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posZselfTestDataset.finaCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posZselfTestDataset.finaCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posZselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posZselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posZselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + +#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1 + sif::info << "IMTQ self test (INIT) err: " + << static_cast(posZselfTestDataset.initErr.value) << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field X: " << posZselfTestDataset.initRawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << posZselfTestDataset.initRawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << posZselfTestDataset.initRawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " + << posZselfTestDataset.initCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " + << posZselfTestDataset.initCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " + << posZselfTestDataset.initCalMagZ << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) coil X current: " << posZselfTestDataset.initCoilXCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y current: " << posZselfTestDataset.initCoilYCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z current: " << posZselfTestDataset.initCoilZCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil X temperature: " + << posZselfTestDataset.initCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y temperature: " + << posZselfTestDataset.initCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z temperature: " + << posZselfTestDataset.initCoilZTemperature << " °C" << std::endl; + + sif::info << "IMTQ self test (+Z) err: " + << static_cast(posZselfTestDataset.err.value) << std::endl; + sif::info << "IMTQ self test (+Z) raw magnetic field X: " << posZselfTestDataset.rawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (+Z) raw magnetic field Y: " << posZselfTestDataset.rawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (+Z) raw magnetic field Z: " << posZselfTestDataset.rawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (+Z) calibrated magnetic field X: " << posZselfTestDataset.calMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (+Z) calibrated magnetic field Y: " << posZselfTestDataset.calMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (+Z) calibrated magnetic field Z: " << posZselfTestDataset.calMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (+Z) coil X current: " << posZselfTestDataset.coilXCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (+Z) coil Y current: " << posZselfTestDataset.coilYCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (+Z) coil Z current: " << posZselfTestDataset.coilZCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (+Z) coil X temperature: " << posZselfTestDataset.coilXTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (+Z) coil Y temperature: " << posZselfTestDataset.coilYTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (+Z) coil Z temperature: " << negYselfTestDataset.coilZTemperature + << " °C" << std::endl; + + sif::info << "IMTQ self test (FINA) err: " + << static_cast(posZselfTestDataset.finaErr.value) << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field X: " << posZselfTestDataset.finaRawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << posZselfTestDataset.finaRawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << posZselfTestDataset.finaRawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " + << posZselfTestDataset.finaCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " + << posZselfTestDataset.finaCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " + << posZselfTestDataset.finaCalMagZ << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) coil X current: " << posZselfTestDataset.finaCoilXCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y current: " << posZselfTestDataset.finaCoilYCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z current: " << posZselfTestDataset.finaCoilZCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil X temperature: " + << posZselfTestDataset.finaCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y temperature: " + << posZselfTestDataset.finaCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z temperature: " + << posZselfTestDataset.finaCoilZTemperature << " °C" << std::endl; +#endif +} + +void IMTQHandler::handleNegativeZSelfTestReply(const uint8_t* packet) { + PoolReadGuard rg(&posXselfTestDataset); + + uint16_t offset = 2; + /** Init measurements */ + negZselfTestDataset.initErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + negZselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negZselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negZselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negZselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negZselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negZselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negZselfTestDataset.initCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negZselfTestDataset.initCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negZselfTestDataset.initCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negZselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negZselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negZselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** +X measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + negZselfTestDataset.err = *(packet + offset); + offset += 2; // STEP byte will not be stored + negZselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negZselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negZselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negZselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negZselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negZselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negZselfTestDataset.coilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negZselfTestDataset.coilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negZselfTestDataset.coilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negZselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negZselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negZselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** FINA measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + negZselfTestDataset.finaErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + negZselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negZselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negZselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negZselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negZselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negZselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negZselfTestDataset.finaCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negZselfTestDataset.finaCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negZselfTestDataset.finaCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negZselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negZselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negZselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + +#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1 + sif::info << "IMTQ self test (INIT) err: " + << static_cast(negZselfTestDataset.initErr.value) << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field X: " << negZselfTestDataset.initRawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << negZselfTestDataset.initRawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << negZselfTestDataset.initRawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " + << negZselfTestDataset.initCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " + << negZselfTestDataset.initCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " + << negZselfTestDataset.initCalMagZ << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) coil X current: " << negZselfTestDataset.initCoilXCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y current: " << negZselfTestDataset.initCoilYCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z current: " << negZselfTestDataset.initCoilZCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil X temperature: " + << negZselfTestDataset.initCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y temperature: " + << negZselfTestDataset.initCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z temperature: " + << negZselfTestDataset.initCoilZTemperature << " °C" << std::endl; + + sif::info << "IMTQ self test (-Z) err: " + << static_cast(negZselfTestDataset.err.value) << std::endl; + sif::info << "IMTQ self test (-Z) raw magnetic field X: " << negZselfTestDataset.rawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (-Z) raw magnetic field Y: " << negZselfTestDataset.rawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (-Z) raw magnetic field Z: " << negZselfTestDataset.rawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (-Z) calibrated magnetic field X: " << negZselfTestDataset.calMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (-Z) calibrated magnetic field Y: " << negZselfTestDataset.calMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (-Z) calibrated magnetic field Z: " << negZselfTestDataset.calMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (-Z) coil X current: " << negZselfTestDataset.coilXCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (-Z) coil Y current: " << negZselfTestDataset.coilYCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (-Z) coil Z current: " << negZselfTestDataset.coilZCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (-Z) coil X temperature: " << negZselfTestDataset.coilXTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (-Z) coil Y temperature: " << negZselfTestDataset.coilYTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (-Z) coil Z temperature: " << negYselfTestDataset.coilZTemperature + << " °C" << std::endl; + + sif::info << "IMTQ self test (FINA) err: " + << static_cast(negZselfTestDataset.finaErr.value) << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field X: " << negZselfTestDataset.finaRawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << negZselfTestDataset.finaRawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << negZselfTestDataset.finaRawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " + << negZselfTestDataset.finaCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " + << negZselfTestDataset.finaCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " + << negZselfTestDataset.finaCalMagZ << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) coil X current: " << negZselfTestDataset.finaCoilXCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y current: " << negZselfTestDataset.finaCoilYCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z current: " << negZselfTestDataset.finaCoilZCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil X temperature: " + << negZselfTestDataset.finaCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y temperature: " + << negZselfTestDataset.finaCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z temperature: " + << negZselfTestDataset.finaCoilZTemperature << " °C" << std::endl; +#endif +} + void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) { std::string stepString(""); if (step < 8) { stepString = makeStepString(step); - } - else { + } else { /** This should normally never happen */ sif::debug << "IMTQHandler::checkErrorByte: Invalid step" << std::endl; return; @@ -912,7 +2121,7 @@ std::string IMTQHandler::makeStepString(const uint8_t step) { break; default: sif::error << "IMTQHandler::checkErrorByte: Received packet with invalid step information" - << std::endl; + << std::endl; break; } return stepString; diff --git a/mission/devices/IMTQHandler.h b/mission/devices/IMTQHandler.h index 92d24a46..ce9e45e0 100644 --- a/mission/devices/IMTQHandler.h +++ b/mission/devices/IMTQHandler.h @@ -82,7 +82,12 @@ private: IMTQ::EngHkDataset engHkDataset; IMTQ::CalibratedMtmMeasurementSet calMtmMeasurementSet; IMTQ::RawMtmMeasurementSet rawMtmMeasurementSet; - IMTQ::PosXselfTestSet posXselfTestDataset; + IMTQ::PosXSelfTestSet posXselfTestDataset; + IMTQ::NegXSelfTestSet negXselfTestDataset; + IMTQ::PosYSelfTestSet posYselfTestDataset; + IMTQ::NegYSelfTestSet negYselfTestDataset; + IMTQ::PosZSelfTestSet posZselfTestDataset; + IMTQ::NegZSelfTestSet negZselfTestDataset; uint8_t commandBuffer[IMTQ::MAX_COMMAND_SIZE]; @@ -170,6 +175,11 @@ private: * @param packet Pointer to the reply data holding the self test result. */ void handlePositiveXSelfTestReply(const uint8_t* packet); + void handleNegativeXSelfTestReply(const uint8_t* packet); + void handlePositiveYSelfTestReply(const uint8_t* packet); + void handleNegativeYSelfTestReply(const uint8_t* packet); + void handlePositiveZSelfTestReply(const uint8_t* packet); + void handleNegativeZSelfTestReply(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 d90a41b0..e95bd42e 100644 --- a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h +++ b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h @@ -29,12 +29,13 @@ namespace IMTQ { static const uint32_t ENG_HK_DATA_SET_ID = 1; static const uint32_t CAL_MTM_SET = 2; - static const uint32_t POS_X_TEST_DATASET = 3; - static const uint32_t NEG_X_TEST_DATASET = 4; - static const uint32_t POS_Y_TEST_DATASET = 5; - static const uint32_t NEG_Y_TEST_DATASET = 6; - static const uint32_t POS_Z_TEST_DATASET = 7; - static const uint32_t NEG_Z_TEST_DATASET = 8; + static const uint32_t RAW_MTM_SET = 3; + static const uint32_t POS_X_TEST_DATASET = 4; + static const uint32_t NEG_X_TEST_DATASET = 5; + static const uint32_t POS_Y_TEST_DATASET = 6; + static const uint32_t NEG_Y_TEST_DATASET = 7; + static const uint32_t POS_Z_TEST_DATASET = 8; + static const uint32_t NEG_Z_TEST_DATASET = 9; static const uint8_t SIZE_ENG_HK_COMMAND = 1; static const uint8_t SIZE_STATUS_REPLY = 2; @@ -405,7 +406,7 @@ public: }; /** - * @brief This dataset holds the raw MTM measurements. + * @brief This dataset holds the last calibrated MTM measurement. */ class CalibratedMtmMeasurementSet: public StaticLocalDataSet { public: @@ -428,17 +429,17 @@ public: }; /** - * @brief This dataset holds the last calibrated MTM measurement. + * @brief This dataset holds the raw MTM measurements. */ class RawMtmMeasurementSet: public StaticLocalDataSet { public: RawMtmMeasurementSet(HasLocalDataPoolIF* owner) : - StaticLocalDataSet(owner, CAL_MTM_SET) { + StaticLocalDataSet(owner, RAW_MTM_SET) { } RawMtmMeasurementSet(object_id_t objectId) : - StaticLocalDataSet(sid_t(objectId, CAL_MTM_SET)) { + StaticLocalDataSet(sid_t(objectId, RAW_MTM_SET)) { } /** The unit of all measurements is nT */ @@ -450,29 +451,71 @@ public: this); }; + /** - * @brief This dataset can be used to store the self test results of the positve X axis test. + * @brief This class can be used to ease the generation of an action message commanding the + * IMTQHandler to configure the magnettorquer with the desired dipoles. + * + * @details Deserialize the packet, write the deserialized data to the ipc store and store the + * the ipc store address in the action message. + */ +class CommandDipolePacket : public SerialLinkedListAdapter { +public: + + CommandDipolePacket() { + setLinks(); + } + +private: + + /** + * @brief Constructor + * + * @param xDipole The dipole of the x coil in 10 ^ -4 * Am^2 + * @param yDipole The dipole of the y coil in 10 ^ -4 * Am^2 + * @param zDipole The dipole of the z coil in 10 ^ -4 * Am^2 + * @param duration The duration in milliseconds the dipole will be generated by the coils. + * When set to 0, the dipole will be generated until a new dipole actuation + * command is sent. + */ + CommandDipolePacket(uint16_t xDipole, uint16_t yDipole, uint16_t zDipole, uint16_t duration) : + xDipole(xDipole), yDipole(yDipole), zDipole(zDipole), duration(duration) { + } + void setLinks() { + setStart(&xDipole); + xDipole.setNext(&yDipole); + yDipole.setNext(&zDipole); + zDipole.setNext(&duration); + } + SerializeElement xDipole; + SerializeElement yDipole; + SerializeElement zDipole; + SerializeElement duration; +}; + +/** + * @brief This dataset can be used to store the self test results of the +X self test. * * @details Units of measurements: * Raw magnetic field: [nT] * Calibrated magnetic field: [nT] * Coil currents: [mA] * 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: + * The +X self test generates a positive dipole in X direction 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 (apply current to generate dipole in positive X direction) + * 2. +X actuation * 3. All coils off (FINA step) */ -class PosXselfTestSet: public StaticLocalDataSet { +class PosXSelfTestSet: public StaticLocalDataSet { public: - PosXselfTestSet(HasLocalDataPoolIF* owner) : - StaticLocalDataSet(owner, POS_X_TEST_DATASET) { + PosXSelfTestSet(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, IMTQ::POS_X_TEST_DATASET) { } - PosXselfTestSet(object_id_t objectId) : - StaticLocalDataSet(sid_t(objectId, POS_X_TEST_DATASET)) { + PosXSelfTestSet(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, IMTQ::POS_X_TEST_DATASET)) { } /** INIT block */ @@ -540,45 +583,451 @@ public: }; /** - * @brief This class can be used to ease the generation of an action message commanding the - * IMTQHandler to configure the magnettorquer with the desired dipoles. + * @brief This dataset can be used to store the self test results of the -X self test. * - * @details Deserialize the packet, write the deserialized data to the ipc store and store the - * the ipc store address in the action message. + * @details Units of measurements: + * Raw magnetic field: [nT] + * Calibrated magnetic field: [nT] + * Coil currents: [mA] + * Temperature: [°C] + * The -X self test generates a negative dipole in X direction 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 actuation + * 3. All coils off (FINA step) */ -class CommandDipolePacket : public SerialLinkedListAdapter { +class NegXSelfTestSet: public StaticLocalDataSet { public: - CommandDipolePacket() { - setLinks(); + NegXSelfTestSet(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, IMTQ::NEG_X_TEST_DATASET) { } -private: + NegXSelfTestSet(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, IMTQ::NEG_X_TEST_DATASET)) { + } - /** - * @brief Constructor - * - * @param xDipole The dipole of the x coil in 10 ^ -4 * Am^2 - * @param yDipole The dipole of the y coil in 10 ^ -4 * Am^2 - * @param zDipole The dipole of the z coil in 10 ^ -4 * Am^2 - * @param duration The duration in milliseconds the dipole will be generated by the coils. - * When set to 0, the dipole will be generated until a new dipole actuation - * command is sent. - */ - CommandDipolePacket(uint16_t xDipole, uint16_t yDipole, uint16_t zDipole, uint16_t duration) : - xDipole(xDipole), yDipole(yDipole), zDipole(zDipole), duration(duration) { - } - void setLinks() { - setStart(&xDipole); - xDipole.setNext(&yDipole); - yDipole.setNext(&zDipole); - zDipole.setNext(&duration); - } - SerializeElement xDipole; - SerializeElement yDipole; - SerializeElement zDipole; - SerializeElement duration; + /** INIT block */ + lp_var_t initErr = lp_var_t(sid.objectId, INIT_NEG_X_ERR, this); + lp_var_t initRawMagX = lp_var_t(sid.objectId, INIT_NEG_X_RAW_MAG_X, this); + lp_var_t initRawMagY = lp_var_t(sid.objectId, INIT_NEG_X_RAW_MAG_Y, this); + lp_var_t initRawMagZ = lp_var_t(sid.objectId, INIT_NEG_X_RAW_MAG_Z, this); + lp_var_t initCalMagX = lp_var_t(sid.objectId, INIT_NEG_X_CAL_MAG_X, this); + lp_var_t initCalMagY = lp_var_t(sid.objectId, INIT_NEG_X_CAL_MAG_Y, this); + lp_var_t initCalMagZ = lp_var_t(sid.objectId, INIT_NEG_X_CAL_MAG_Z, this); + lp_var_t initCoilXCurrent = lp_var_t(sid.objectId, INIT_NEG_X_COIL_X_CURRENT, + this); + lp_var_t initCoilYCurrent = lp_var_t(sid.objectId, INIT_NEG_X_COIL_Y_CURRENT, + this); + lp_var_t initCoilZCurrent = lp_var_t(sid.objectId, INIT_NEG_X_COIL_Z_CURRENT, + this); + lp_var_t initCoilXTemperature = lp_var_t(sid.objectId, + INIT_NEG_X_COIL_X_TEMPERATURE, this); + lp_var_t initCoilYTemperature = lp_var_t(sid.objectId, + INIT_NEG_X_COIL_Y_TEMPERATURE, this); + lp_var_t initCoilZTemperature = lp_var_t(sid.objectId, + INIT_NEG_X_COIL_Z_TEMPERATURE, this); + + /** -X block */ + lp_var_t err = lp_var_t(sid.objectId, NEG_X_ERR, this); + lp_var_t rawMagX = lp_var_t(sid.objectId, NEG_X_RAW_MAG_X, this); + lp_var_t rawMagY = lp_var_t(sid.objectId, NEG_X_RAW_MAG_Y, this); + lp_var_t rawMagZ = lp_var_t(sid.objectId, NEG_X_RAW_MAG_Z, this); + lp_var_t calMagX = lp_var_t(sid.objectId, NEG_X_CAL_MAG_X, this); + lp_var_t calMagY = lp_var_t(sid.objectId, NEG_X_CAL_MAG_Y, this); + lp_var_t calMagZ = lp_var_t(sid.objectId, NEG_X_CAL_MAG_Z, this); + lp_var_t coilXCurrent = lp_var_t(sid.objectId, NEG_X_COIL_X_CURRENT, + this); + lp_var_t coilYCurrent = lp_var_t(sid.objectId, NEG_X_COIL_Y_CURRENT, + this); + lp_var_t coilZCurrent = lp_var_t(sid.objectId, NEG_X_COIL_Z_CURRENT, + this); + lp_var_t coilXTemperature = lp_var_t(sid.objectId, + NEG_X_COIL_X_TEMPERATURE, this); + lp_var_t coilYTemperature = lp_var_t(sid.objectId, + NEG_X_COIL_Y_TEMPERATURE, this); + lp_var_t coilZTemperature = lp_var_t(sid.objectId, + NEG_X_COIL_Z_TEMPERATURE, this); + + /** FINA block */ + lp_var_t finaErr = lp_var_t(sid.objectId, FINA_NEG_X_ERR, this); + lp_var_t finaRawMagX = lp_var_t(sid.objectId, FINA_NEG_X_RAW_MAG_X, this); + lp_var_t finaRawMagY = lp_var_t(sid.objectId, FINA_NEG_X_RAW_MAG_Y, this); + lp_var_t finaRawMagZ = lp_var_t(sid.objectId, FINA_NEG_X_RAW_MAG_Z, this); + lp_var_t finaCalMagX = lp_var_t(sid.objectId, FINA_NEG_X_CAL_MAG_X, this); + lp_var_t finaCalMagY = lp_var_t(sid.objectId, FINA_NEG_X_CAL_MAG_Y, this); + lp_var_t finaCalMagZ = lp_var_t(sid.objectId, FINA_NEG_X_CAL_MAG_Z, this); + lp_var_t finaCoilXCurrent = lp_var_t(sid.objectId, FINA_NEG_X_COIL_X_CURRENT, + this); + lp_var_t finaCoilYCurrent = lp_var_t(sid.objectId, FINA_NEG_X_COIL_Y_CURRENT, + this); + lp_var_t finaCoilZCurrent = lp_var_t(sid.objectId, FINA_NEG_X_COIL_Z_CURRENT, + this); + lp_var_t finaCoilXTemperature = lp_var_t(sid.objectId, + FINA_NEG_X_COIL_X_TEMPERATURE, this); + lp_var_t finaCoilYTemperature = lp_var_t(sid.objectId, + FINA_NEG_X_COIL_Y_TEMPERATURE, this); + lp_var_t finaCoilZTemperature = lp_var_t(sid.objectId, + FINA_NEG_X_COIL_Z_TEMPERATURE, this); }; + + +/** + * @brief This dataset can be used to store the self test results of the +Y self test. + * + * @details Units of measurements: + * Raw magnetic field: [nT] + * Calibrated magnetic field: [nT] + * Coil currents: [mA] + * Temperature: [°C] + * The +Y self test generates a positive dipole in y direction 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. +Y actuation + * 3. All coils off (FINA step) + */ +class PosYSelfTestSet: public StaticLocalDataSet { +public: + + PosYSelfTestSet(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, IMTQ::POS_Y_TEST_DATASET) { + } + + PosYSelfTestSet(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, IMTQ::POS_Y_TEST_DATASET)) { + } + + /** INIT block */ + lp_var_t initErr = lp_var_t(sid.objectId, INIT_POS_Y_ERR, this); + lp_var_t initRawMagX = lp_var_t(sid.objectId, INIT_POS_Y_RAW_MAG_X, this); + lp_var_t initRawMagY = lp_var_t(sid.objectId, INIT_POS_Y_RAW_MAG_Y, this); + lp_var_t initRawMagZ = lp_var_t(sid.objectId, INIT_POS_Y_RAW_MAG_Z, this); + lp_var_t initCalMagX = lp_var_t(sid.objectId, INIT_POS_Y_CAL_MAG_X, this); + lp_var_t initCalMagY = lp_var_t(sid.objectId, INIT_POS_Y_CAL_MAG_Y, this); + lp_var_t initCalMagZ = lp_var_t(sid.objectId, INIT_POS_Y_CAL_MAG_Z, this); + lp_var_t initCoilXCurrent = lp_var_t(sid.objectId, INIT_POS_Y_COIL_X_CURRENT, + this); + lp_var_t initCoilYCurrent = lp_var_t(sid.objectId, INIT_POS_Y_COIL_Y_CURRENT, + this); + lp_var_t initCoilZCurrent = lp_var_t(sid.objectId, INIT_POS_Y_COIL_Z_CURRENT, + this); + lp_var_t initCoilXTemperature = lp_var_t(sid.objectId, + INIT_POS_Y_COIL_X_TEMPERATURE, this); + lp_var_t initCoilYTemperature = lp_var_t(sid.objectId, + INIT_POS_Y_COIL_Y_TEMPERATURE, this); + lp_var_t initCoilZTemperature = lp_var_t(sid.objectId, + INIT_POS_Y_COIL_Z_TEMPERATURE, this); + + /** +Y block */ + lp_var_t err = lp_var_t(sid.objectId, POS_Y_ERR, this); + lp_var_t rawMagX = lp_var_t(sid.objectId, POS_Y_RAW_MAG_X, this); + lp_var_t rawMagY = lp_var_t(sid.objectId, POS_Y_RAW_MAG_Y, this); + lp_var_t rawMagZ = lp_var_t(sid.objectId, POS_Y_RAW_MAG_Z, this); + lp_var_t calMagX = lp_var_t(sid.objectId, POS_Y_CAL_MAG_X, this); + lp_var_t calMagY = lp_var_t(sid.objectId, POS_Y_CAL_MAG_Y, this); + lp_var_t calMagZ = lp_var_t(sid.objectId, POS_Y_CAL_MAG_Z, this); + lp_var_t coilXCurrent = lp_var_t(sid.objectId, POS_Y_COIL_X_CURRENT, + this); + lp_var_t coilYCurrent = lp_var_t(sid.objectId, POS_Y_COIL_Y_CURRENT, + this); + lp_var_t coilZCurrent = lp_var_t(sid.objectId, POS_Y_COIL_Z_CURRENT, + this); + lp_var_t coilXTemperature = lp_var_t(sid.objectId, + POS_Y_COIL_X_TEMPERATURE, this); + lp_var_t coilYTemperature = lp_var_t(sid.objectId, + POS_Y_COIL_Y_TEMPERATURE, this); + lp_var_t coilZTemperature = lp_var_t(sid.objectId, + POS_Y_COIL_Z_TEMPERATURE, this); + + /** FINA block */ + lp_var_t finaErr = lp_var_t(sid.objectId, FINA_POS_Y_ERR, this); + lp_var_t finaRawMagX = lp_var_t(sid.objectId, FINA_POS_Y_RAW_MAG_X, this); + lp_var_t finaRawMagY = lp_var_t(sid.objectId, FINA_POS_Y_RAW_MAG_Y, this); + lp_var_t finaRawMagZ = lp_var_t(sid.objectId, FINA_POS_Y_RAW_MAG_Z, this); + lp_var_t finaCalMagX = lp_var_t(sid.objectId, FINA_POS_Y_CAL_MAG_X, this); + lp_var_t finaCalMagY = lp_var_t(sid.objectId, FINA_POS_Y_CAL_MAG_Y, this); + lp_var_t finaCalMagZ = lp_var_t(sid.objectId, FINA_POS_Y_CAL_MAG_Z, this); + lp_var_t finaCoilXCurrent = lp_var_t(sid.objectId, FINA_POS_Y_COIL_X_CURRENT, + this); + lp_var_t finaCoilYCurrent = lp_var_t(sid.objectId, FINA_POS_Y_COIL_Y_CURRENT, + this); + lp_var_t finaCoilZCurrent = lp_var_t(sid.objectId, FINA_POS_Y_COIL_Z_CURRENT, + this); + lp_var_t finaCoilXTemperature = lp_var_t(sid.objectId, + FINA_POS_Y_COIL_X_TEMPERATURE, this); + lp_var_t finaCoilYTemperature = lp_var_t(sid.objectId, + FINA_POS_Y_COIL_Y_TEMPERATURE, this); + lp_var_t finaCoilZTemperature = lp_var_t(sid.objectId, + FINA_POS_Y_COIL_Z_TEMPERATURE, this); +}; + +/** + * @brief This dataset can be used to store the self test results of the -Y self test. + * + * @details Units of measurements: + * Raw magnetic field: [nT] + * Calibrated magnetic field: [nT] + * Coil currents: [mA] + * Temperature: [°C] + * The -Y self test generates a negative dipole in y direction 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. -Y actuation + * 3. All coils off (FINA step) + */ +class NegYSelfTestSet: public StaticLocalDataSet { +public: + + NegYSelfTestSet(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, IMTQ::NEG_Y_TEST_DATASET) { + } + + NegYSelfTestSet(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, IMTQ::NEG_Y_TEST_DATASET)) { + } + + /** INIT block */ + lp_var_t initErr = lp_var_t(sid.objectId, INIT_NEG_Y_ERR, this); + lp_var_t initRawMagX = lp_var_t(sid.objectId, INIT_NEG_Y_RAW_MAG_X, this); + lp_var_t initRawMagY = lp_var_t(sid.objectId, INIT_NEG_Y_RAW_MAG_Y, this); + lp_var_t initRawMagZ = lp_var_t(sid.objectId, INIT_NEG_Y_RAW_MAG_Z, this); + lp_var_t initCalMagX = lp_var_t(sid.objectId, INIT_NEG_Y_CAL_MAG_X, this); + lp_var_t initCalMagY = lp_var_t(sid.objectId, INIT_NEG_Y_CAL_MAG_Y, this); + lp_var_t initCalMagZ = lp_var_t(sid.objectId, INIT_NEG_Y_CAL_MAG_Z, this); + lp_var_t initCoilXCurrent = lp_var_t(sid.objectId, INIT_NEG_Y_COIL_X_CURRENT, + this); + lp_var_t initCoilYCurrent = lp_var_t(sid.objectId, INIT_NEG_Y_COIL_Y_CURRENT, + this); + lp_var_t initCoilZCurrent = lp_var_t(sid.objectId, INIT_NEG_Y_COIL_Z_CURRENT, + this); + lp_var_t initCoilXTemperature = lp_var_t(sid.objectId, + INIT_NEG_Y_COIL_X_TEMPERATURE, this); + lp_var_t initCoilYTemperature = lp_var_t(sid.objectId, + INIT_NEG_Y_COIL_Y_TEMPERATURE, this); + lp_var_t initCoilZTemperature = lp_var_t(sid.objectId, + INIT_NEG_Y_COIL_Z_TEMPERATURE, this); + + /** -Y block */ + lp_var_t err = lp_var_t(sid.objectId, NEG_Y_ERR, this); + lp_var_t rawMagX = lp_var_t(sid.objectId, NEG_Y_RAW_MAG_X, this); + lp_var_t rawMagY = lp_var_t(sid.objectId, NEG_Y_RAW_MAG_Y, this); + lp_var_t rawMagZ = lp_var_t(sid.objectId, NEG_Y_RAW_MAG_Z, this); + lp_var_t calMagX = lp_var_t(sid.objectId, NEG_Y_CAL_MAG_X, this); + lp_var_t calMagY = lp_var_t(sid.objectId, NEG_Y_CAL_MAG_Y, this); + lp_var_t calMagZ = lp_var_t(sid.objectId, NEG_Y_CAL_MAG_Z, this); + lp_var_t coilXCurrent = lp_var_t(sid.objectId, NEG_Y_COIL_X_CURRENT, + this); + lp_var_t coilYCurrent = lp_var_t(sid.objectId, NEG_Y_COIL_Y_CURRENT, + this); + lp_var_t coilZCurrent = lp_var_t(sid.objectId, NEG_Y_COIL_Z_CURRENT, + this); + lp_var_t coilXTemperature = lp_var_t(sid.objectId, + NEG_Y_COIL_X_TEMPERATURE, this); + lp_var_t coilYTemperature = lp_var_t(sid.objectId, + NEG_Y_COIL_Y_TEMPERATURE, this); + lp_var_t coilZTemperature = lp_var_t(sid.objectId, + NEG_Y_COIL_Z_TEMPERATURE, this); + + /** FINA block */ + lp_var_t finaErr = lp_var_t(sid.objectId, FINA_NEG_Y_ERR, this); + lp_var_t finaRawMagX = lp_var_t(sid.objectId, FINA_NEG_Y_RAW_MAG_X, this); + lp_var_t finaRawMagY = lp_var_t(sid.objectId, FINA_NEG_Y_RAW_MAG_Y, this); + lp_var_t finaRawMagZ = lp_var_t(sid.objectId, FINA_NEG_Y_RAW_MAG_Z, this); + lp_var_t finaCalMagX = lp_var_t(sid.objectId, FINA_NEG_Y_CAL_MAG_X, this); + lp_var_t finaCalMagY = lp_var_t(sid.objectId, FINA_NEG_Y_CAL_MAG_Y, this); + lp_var_t finaCalMagZ = lp_var_t(sid.objectId, FINA_NEG_Y_CAL_MAG_Z, this); + lp_var_t finaCoilXCurrent = lp_var_t(sid.objectId, FINA_NEG_Y_COIL_X_CURRENT, + this); + lp_var_t finaCoilYCurrent = lp_var_t(sid.objectId, FINA_NEG_Y_COIL_Y_CURRENT, + this); + lp_var_t finaCoilZCurrent = lp_var_t(sid.objectId, FINA_NEG_Y_COIL_Z_CURRENT, + this); + lp_var_t finaCoilXTemperature = lp_var_t(sid.objectId, + FINA_NEG_Y_COIL_X_TEMPERATURE, this); + lp_var_t finaCoilYTemperature = lp_var_t(sid.objectId, + FINA_NEG_Y_COIL_Y_TEMPERATURE, this); + lp_var_t finaCoilZTemperature = lp_var_t(sid.objectId, + FINA_NEG_Y_COIL_Z_TEMPERATURE, this); +}; + +/** + * @brief This dataset can be used to store the self test results of the +Z self test. + * + * @details Units of measurements: + * Raw magnetic field: [nT] + * Calibrated magnetic field: [nT] + * Coil currents: [mA] + * Temperature: [°C] + * The +Z self test generates a positive dipole in z direction 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. +Z actuation + * 3. All coils off (FINA step) + */ +class PosZSelfTestSet: public StaticLocalDataSet { +public: + + PosZSelfTestSet(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, IMTQ::POS_Z_TEST_DATASET) { + } + + PosZSelfTestSet(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, IMTQ::POS_Z_TEST_DATASET)) { + } + + /** INIT block */ + lp_var_t initErr = lp_var_t(sid.objectId, INIT_POS_Z_ERR, this); + lp_var_t initRawMagX = lp_var_t(sid.objectId, INIT_POS_Z_RAW_MAG_X, this); + lp_var_t initRawMagY = lp_var_t(sid.objectId, INIT_POS_Z_RAW_MAG_Y, this); + lp_var_t initRawMagZ = lp_var_t(sid.objectId, INIT_POS_Z_RAW_MAG_Z, this); + lp_var_t initCalMagX = lp_var_t(sid.objectId, INIT_POS_Z_CAL_MAG_X, this); + lp_var_t initCalMagY = lp_var_t(sid.objectId, INIT_POS_Z_CAL_MAG_Y, this); + lp_var_t initCalMagZ = lp_var_t(sid.objectId, INIT_POS_Z_CAL_MAG_Z, this); + lp_var_t initCoilXCurrent = lp_var_t(sid.objectId, INIT_POS_Z_COIL_X_CURRENT, + this); + lp_var_t initCoilYCurrent = lp_var_t(sid.objectId, INIT_POS_Z_COIL_Y_CURRENT, + this); + lp_var_t initCoilZCurrent = lp_var_t(sid.objectId, INIT_POS_Z_COIL_Z_CURRENT, + this); + lp_var_t initCoilXTemperature = lp_var_t(sid.objectId, + INIT_POS_Z_COIL_X_TEMPERATURE, this); + lp_var_t initCoilYTemperature = lp_var_t(sid.objectId, + INIT_POS_Z_COIL_Y_TEMPERATURE, this); + lp_var_t initCoilZTemperature = lp_var_t(sid.objectId, + INIT_POS_Z_COIL_Z_TEMPERATURE, this); + + /** +Z block */ + lp_var_t err = lp_var_t(sid.objectId, POS_Z_ERR, this); + lp_var_t rawMagX = lp_var_t(sid.objectId, POS_Z_RAW_MAG_X, this); + lp_var_t rawMagY = lp_var_t(sid.objectId, POS_Z_RAW_MAG_Y, this); + lp_var_t rawMagZ = lp_var_t(sid.objectId, POS_Z_RAW_MAG_Z, this); + lp_var_t calMagX = lp_var_t(sid.objectId, POS_Z_CAL_MAG_X, this); + lp_var_t calMagY = lp_var_t(sid.objectId, POS_Z_CAL_MAG_Y, this); + lp_var_t calMagZ = lp_var_t(sid.objectId, POS_Z_CAL_MAG_Z, this); + lp_var_t coilXCurrent = lp_var_t(sid.objectId, POS_Z_COIL_X_CURRENT, + this); + lp_var_t coilYCurrent = lp_var_t(sid.objectId, POS_Z_COIL_Y_CURRENT, + this); + lp_var_t coilZCurrent = lp_var_t(sid.objectId, POS_Z_COIL_Z_CURRENT, + this); + lp_var_t coilXTemperature = lp_var_t(sid.objectId, + POS_Z_COIL_X_TEMPERATURE, this); + lp_var_t coilYTemperature = lp_var_t(sid.objectId, + POS_Z_COIL_Y_TEMPERATURE, this); + lp_var_t coilZTemperature = lp_var_t(sid.objectId, + POS_Z_COIL_Z_TEMPERATURE, this); + + /** FINA block */ + lp_var_t finaErr = lp_var_t(sid.objectId, FINA_POS_Z_ERR, this); + lp_var_t finaRawMagX = lp_var_t(sid.objectId, FINA_POS_Z_RAW_MAG_X, this); + lp_var_t finaRawMagY = lp_var_t(sid.objectId, FINA_POS_Z_RAW_MAG_Y, this); + lp_var_t finaRawMagZ = lp_var_t(sid.objectId, FINA_POS_Z_RAW_MAG_Z, this); + lp_var_t finaCalMagX = lp_var_t(sid.objectId, FINA_POS_Z_CAL_MAG_X, this); + lp_var_t finaCalMagY = lp_var_t(sid.objectId, FINA_POS_Z_CAL_MAG_Y, this); + lp_var_t finaCalMagZ = lp_var_t(sid.objectId, FINA_POS_Z_CAL_MAG_Z, this); + lp_var_t finaCoilXCurrent = lp_var_t(sid.objectId, FINA_POS_Z_COIL_X_CURRENT, + this); + lp_var_t finaCoilYCurrent = lp_var_t(sid.objectId, FINA_POS_Z_COIL_Y_CURRENT, + this); + lp_var_t finaCoilZCurrent = lp_var_t(sid.objectId, FINA_POS_Z_COIL_Z_CURRENT, + this); + lp_var_t finaCoilXTemperature = lp_var_t(sid.objectId, + FINA_POS_Z_COIL_X_TEMPERATURE, this); + lp_var_t finaCoilYTemperature = lp_var_t(sid.objectId, + FINA_POS_Z_COIL_Y_TEMPERATURE, this); + lp_var_t finaCoilZTemperature = lp_var_t(sid.objectId, + FINA_POS_Z_COIL_Z_TEMPERATURE, this); +}; + +/** + * @brief This dataset can be used to store the self test results of the -Z self test. + * + * @details Units of measurements: + * Raw magnetic field: [nT] + * Calibrated magnetic field: [nT] + * Coil currents: [mA] + * Temperature: [°C] + * The -Z self test generates a negative dipole in z direction 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. -Z actuation + * 3. All coils off (FINA step) + */ +class NegZSelfTestSet: public StaticLocalDataSet { +public: + + NegZSelfTestSet(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, IMTQ::NEG_Z_TEST_DATASET) { + } + + NegZSelfTestSet(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, IMTQ::NEG_Z_TEST_DATASET)) { + } + + /** INIT block */ + lp_var_t initErr = lp_var_t(sid.objectId, INIT_NEG_Z_ERR, this); + lp_var_t initRawMagX = lp_var_t(sid.objectId, INIT_NEG_Z_RAW_MAG_X, this); + lp_var_t initRawMagY = lp_var_t(sid.objectId, INIT_NEG_Z_RAW_MAG_Y, this); + lp_var_t initRawMagZ = lp_var_t(sid.objectId, INIT_NEG_Z_RAW_MAG_Z, this); + lp_var_t initCalMagX = lp_var_t(sid.objectId, INIT_NEG_Z_CAL_MAG_X, this); + lp_var_t initCalMagY = lp_var_t(sid.objectId, INIT_NEG_Z_CAL_MAG_Y, this); + lp_var_t initCalMagZ = lp_var_t(sid.objectId, INIT_NEG_Z_CAL_MAG_Z, this); + lp_var_t initCoilXCurrent = lp_var_t(sid.objectId, INIT_NEG_Z_COIL_X_CURRENT, + this); + lp_var_t initCoilYCurrent = lp_var_t(sid.objectId, INIT_NEG_Z_COIL_Y_CURRENT, + this); + lp_var_t initCoilZCurrent = lp_var_t(sid.objectId, INIT_NEG_Z_COIL_Z_CURRENT, + this); + lp_var_t initCoilXTemperature = lp_var_t(sid.objectId, + INIT_NEG_Z_COIL_X_TEMPERATURE, this); + lp_var_t initCoilYTemperature = lp_var_t(sid.objectId, + INIT_NEG_Z_COIL_Y_TEMPERATURE, this); + lp_var_t initCoilZTemperature = lp_var_t(sid.objectId, + INIT_NEG_Z_COIL_Z_TEMPERATURE, this); + + /** +Z block */ + lp_var_t err = lp_var_t(sid.objectId, NEG_Z_ERR, this); + lp_var_t rawMagX = lp_var_t(sid.objectId, NEG_Z_RAW_MAG_X, this); + lp_var_t rawMagY = lp_var_t(sid.objectId, NEG_Z_RAW_MAG_Y, this); + lp_var_t rawMagZ = lp_var_t(sid.objectId, NEG_Z_RAW_MAG_Z, this); + lp_var_t calMagX = lp_var_t(sid.objectId, NEG_Z_CAL_MAG_X, this); + lp_var_t calMagY = lp_var_t(sid.objectId, NEG_Z_CAL_MAG_Y, this); + lp_var_t calMagZ = lp_var_t(sid.objectId, NEG_Z_CAL_MAG_Z, this); + lp_var_t coilXCurrent = lp_var_t(sid.objectId, NEG_Z_COIL_X_CURRENT, + this); + lp_var_t coilYCurrent = lp_var_t(sid.objectId, NEG_Z_COIL_Y_CURRENT, + this); + lp_var_t coilZCurrent = lp_var_t(sid.objectId, NEG_Z_COIL_Z_CURRENT, + this); + lp_var_t coilXTemperature = lp_var_t(sid.objectId, + NEG_Z_COIL_X_TEMPERATURE, this); + lp_var_t coilYTemperature = lp_var_t(sid.objectId, + NEG_Z_COIL_Y_TEMPERATURE, this); + lp_var_t coilZTemperature = lp_var_t(sid.objectId, + NEG_Z_COIL_Z_TEMPERATURE, this); + + /** FINA block */ + lp_var_t finaErr = lp_var_t(sid.objectId, FINA_NEG_Z_ERR, this); + lp_var_t finaRawMagX = lp_var_t(sid.objectId, FINA_NEG_Z_RAW_MAG_X, this); + lp_var_t finaRawMagY = lp_var_t(sid.objectId, FINA_NEG_Z_RAW_MAG_Y, this); + lp_var_t finaRawMagZ = lp_var_t(sid.objectId, FINA_NEG_Z_RAW_MAG_Z, this); + lp_var_t finaCalMagX = lp_var_t(sid.objectId, FINA_NEG_Z_CAL_MAG_X, this); + lp_var_t finaCalMagY = lp_var_t(sid.objectId, FINA_NEG_Z_CAL_MAG_Y, this); + lp_var_t finaCalMagZ = lp_var_t(sid.objectId, FINA_NEG_Z_CAL_MAG_Z, this); + lp_var_t finaCoilXCurrent = lp_var_t(sid.objectId, FINA_NEG_Z_COIL_X_CURRENT, + this); + lp_var_t finaCoilYCurrent = lp_var_t(sid.objectId, FINA_NEG_Z_COIL_Y_CURRENT, + this); + lp_var_t finaCoilZCurrent = lp_var_t(sid.objectId, FINA_NEG_Z_COIL_Z_CURRENT, + this); + lp_var_t finaCoilXTemperature = lp_var_t(sid.objectId, + FINA_NEG_Z_COIL_X_TEMPERATURE, this); + lp_var_t finaCoilYTemperature = lp_var_t(sid.objectId, + FINA_NEG_Z_COIL_Y_TEMPERATURE, this); + lp_var_t finaCoilZTemperature = lp_var_t(sid.objectId, + FINA_NEG_Z_COIL_Z_TEMPERATURE, this); +}; + }