eive-obsw/mission/devices/IMTQHandler.cpp

2181 lines
116 KiB
C++
Raw Normal View History

2021-03-17 11:14:48 +01:00
#include "IMTQHandler.h"
2021-03-21 17:35:14 +01:00
#include <fsfw/datapool/PoolReadGuard.h>
2022-01-18 11:41:19 +01:00
#include <fsfw/globalfunctions/CRC.h>
2021-05-17 16:37:29 +02:00
#include <cmath>
2021-03-17 11:14:48 +01:00
2022-01-27 16:04:45 +01:00
#include "OBSWConfig.h"
2022-01-18 11:41:19 +01:00
IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie)
: DeviceHandlerBase(objectId, comIF, comCookie),
engHkDataset(this),
calMtmMeasurementSet(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;
}
2021-03-17 11:14:48 +01:00
}
2022-01-18 11:41:19 +01:00
IMTQHandler::~IMTQHandler() {}
2021-06-15 15:13:47 +02:00
void IMTQHandler::doStartUp() {
if (goToNormalMode) {
setMode(MODE_NORMAL);
} else {
setMode(_MODE_TO_ON);
}
2021-03-17 11:14:48 +01:00
}
2022-01-18 11:41:19 +01:00
void IMTQHandler::doShutDown() { setMode(_MODE_POWER_DOWN); }
2021-03-17 11:14:48 +01:00
2022-01-18 11:41:19 +01:00
ReturnValue_t IMTQHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
switch (communicationStep) {
2021-04-26 07:56:02 +02:00
case CommunicationStep::GET_ENG_HK_DATA:
2022-01-18 11:41:19 +01:00
*id = IMTQ::GET_ENG_HK_DATA;
communicationStep = CommunicationStep::START_MTM_MEASUREMENT;
break;
2021-04-26 07:56:02 +02:00
case CommunicationStep::START_MTM_MEASUREMENT:
2022-01-18 11:41:19 +01:00
*id = IMTQ::START_MTM_MEASUREMENT;
communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT;
break;
2021-04-26 07:56:02 +02:00
case CommunicationStep::GET_CAL_MTM_MEASUREMENT:
2022-01-18 11:41:19 +01:00
*id = IMTQ::GET_CAL_MTM_MEASUREMENT;
communicationStep = CommunicationStep::GET_RAW_MTM_MEASUREMENT;
break;
2021-04-26 07:56:02 +02:00
case CommunicationStep::GET_RAW_MTM_MEASUREMENT:
2022-01-18 11:41:19 +01:00
*id = IMTQ::GET_RAW_MTM_MEASUREMENT;
communicationStep = CommunicationStep::GET_ENG_HK_DATA;
break;
2021-04-26 07:56:02 +02:00
default:
2022-01-18 11:41:19 +01:00
sif::debug << "IMTQHandler::buildNormalDeviceCommand: Invalid communication step"
<< std::endl;
break;
}
return buildCommandFromCommand(*id, NULL, 0);
2021-03-17 11:14:48 +01:00
}
2022-01-18 11:41:19 +01:00
ReturnValue_t IMTQHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { return RETURN_OK; }
2021-03-17 11:14:48 +01:00
2021-06-15 15:13:47 +02:00
ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
2022-01-18 11:41:19 +01:00
const uint8_t* commandData,
size_t commandDataLen) {
switch (deviceCommand) {
2021-06-15 15:13:47 +02:00
case (IMTQ::POS_X_SELF_TEST): {
2022-01-18 11:41:19 +01:00
commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD;
commandBuffer[1] = IMTQ::SELF_TEST_AXIS::X_POSITIVE;
rawPacket = commandBuffer;
rawPacketLen = 2;
return RETURN_OK;
2021-06-15 15:13:47 +02:00
}
case (IMTQ::NEG_X_SELF_TEST): {
2022-01-18 11:41:19 +01:00
commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD;
commandBuffer[1] = IMTQ::SELF_TEST_AXIS::X_NEGATIVE;
rawPacket = commandBuffer;
rawPacketLen = 2;
return RETURN_OK;
2021-06-15 15:13:47 +02:00
}
case (IMTQ::POS_Y_SELF_TEST): {
2022-01-18 11:41:19 +01:00
commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD;
commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Y_POSITIVE;
rawPacket = commandBuffer;
rawPacketLen = 2;
return RETURN_OK;
2021-06-15 15:13:47 +02:00
}
case (IMTQ::NEG_Y_SELF_TEST): {
2022-01-18 11:41:19 +01:00
commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD;
commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Y_NEGATIVE;
rawPacket = commandBuffer;
rawPacketLen = 2;
return RETURN_OK;
2021-06-15 15:13:47 +02:00
}
case (IMTQ::POS_Z_SELF_TEST): {
2022-01-18 11:41:19 +01:00
commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD;
commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Z_POSITIVE;
rawPacket = commandBuffer;
rawPacketLen = 2;
return RETURN_OK;
2021-06-15 15:13:47 +02:00
}
case (IMTQ::NEG_Z_SELF_TEST): {
2022-01-18 11:41:19 +01:00
commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD;
commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Z_NEGATIVE;
rawPacket = commandBuffer;
rawPacketLen = 2;
return RETURN_OK;
2021-06-15 15:13:47 +02:00
}
case (IMTQ::GET_SELF_TEST_RESULT): {
2022-01-18 11:41:19 +01:00
commandBuffer[0] = IMTQ::CC::GET_SELF_TEST_RESULT;
rawPacket = commandBuffer;
rawPacketLen = 1;
return RETURN_OK;
2021-06-15 15:13:47 +02:00
}
case (IMTQ::START_ACTUATION_DIPOLE): {
2022-01-18 11:41:19 +01:00
/* IMTQ expects low byte first */
commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE;
2022-04-14 09:42:59 +02:00
if(commandData == nullptr) {
return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
}
commandBuffer[1] = commandData[1];
commandBuffer[2] = commandData[0];
commandBuffer[3] = commandData[3];
commandBuffer[4] = commandData[2];
commandBuffer[5] = commandData[5];
commandBuffer[6] = commandData[4];
commandBuffer[7] = commandData[7];
commandBuffer[8] = commandData[6];
2022-01-18 11:41:19 +01:00
rawPacket = commandBuffer;
rawPacketLen = 9;
return RETURN_OK;
2021-04-25 15:53:44 +02:00
}
2021-06-15 15:13:47 +02:00
case (IMTQ::GET_ENG_HK_DATA): {
2022-01-18 11:41:19 +01:00
commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA;
rawPacket = commandBuffer;
rawPacketLen = 1;
return RETURN_OK;
2021-06-15 15:13:47 +02:00
}
case (IMTQ::GET_COMMANDED_DIPOLE): {
2022-01-18 11:41:19 +01:00
commandBuffer[0] = IMTQ::CC::GET_COMMANDED_DIPOLE;
rawPacket = commandBuffer;
rawPacketLen = 1;
return RETURN_OK;
2021-06-15 15:13:47 +02:00
}
case (IMTQ::START_MTM_MEASUREMENT): {
2022-01-18 11:41:19 +01:00
commandBuffer[0] = IMTQ::CC::START_MTM_MEASUREMENT;
rawPacket = commandBuffer;
rawPacketLen = 1;
return RETURN_OK;
2021-06-15 15:13:47 +02:00
}
case (IMTQ::GET_CAL_MTM_MEASUREMENT): {
2022-01-18 11:41:19 +01:00
commandBuffer[0] = IMTQ::CC::GET_CAL_MTM_MEASUREMENT;
rawPacket = commandBuffer;
rawPacketLen = 1;
return RETURN_OK;
2021-06-15 15:13:47 +02:00
}
case (IMTQ::GET_RAW_MTM_MEASUREMENT): {
2022-01-18 11:41:19 +01:00
commandBuffer[0] = IMTQ::CC::GET_RAW_MTM_MEASUREMENT;
rawPacket = commandBuffer;
rawPacketLen = 1;
return RETURN_OK;
2021-06-15 15:13:47 +02:00
}
default:
2022-01-18 11:41:19 +01:00
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
return HasReturnvaluesIF::RETURN_FAILED;
2021-03-17 11:14:48 +01:00
}
void IMTQHandler::fillCommandAndReplyMap() {
2022-01-18 11:41:19 +01:00
this->insertInCommandAndReplyMap(IMTQ::POS_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::NEG_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::POS_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::NEG_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::POS_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::NEG_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::GET_SELF_TEST_RESULT, 1, nullptr,
IMTQ::SIZE_SELF_TEST_RESULTS);
this->insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr,
IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset,
IMTQ::SIZE_ENG_HK_DATA_REPLY);
this->insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr,
IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY);
this->insertInCommandAndReplyMap(IMTQ::START_MTM_MEASUREMENT, 1, nullptr,
IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::GET_CAL_MTM_MEASUREMENT, 1, &calMtmMeasurementSet,
IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT);
this->insertInCommandAndReplyMap(IMTQ::GET_RAW_MTM_MEASUREMENT, 1, &rawMtmMeasurementSet,
IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT);
2021-03-17 11:14:48 +01:00
}
2022-01-18 11:41:19 +01:00
ReturnValue_t IMTQHandler::scanForReply(const uint8_t* start, size_t remainingSize,
DeviceCommandId_t* foundId, size_t* foundLen) {
ReturnValue_t result = RETURN_OK;
2021-03-17 11:14:48 +01:00
2022-01-18 11:41:19 +01:00
switch (*start) {
2021-06-15 15:13:47 +02:00
case (IMTQ::CC::START_ACTUATION_DIPOLE):
2022-01-18 11:41:19 +01:00
*foundLen = IMTQ::SIZE_STATUS_REPLY;
*foundId = IMTQ::START_ACTUATION_DIPOLE;
break;
2021-06-15 15:13:47 +02:00
case (IMTQ::CC::START_MTM_MEASUREMENT):
2022-01-18 11:41:19 +01:00
*foundLen = IMTQ::SIZE_STATUS_REPLY;
*foundId = IMTQ::START_MTM_MEASUREMENT;
break;
2021-06-15 15:13:47 +02:00
case (IMTQ::CC::GET_ENG_HK_DATA):
2022-01-18 11:41:19 +01:00
*foundLen = IMTQ::SIZE_ENG_HK_DATA_REPLY;
*foundId = IMTQ::GET_ENG_HK_DATA;
break;
2021-06-15 15:13:47 +02:00
case (IMTQ::CC::GET_COMMANDED_DIPOLE):
2022-01-18 11:41:19 +01:00
*foundLen = IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY;
*foundId = IMTQ::GET_COMMANDED_DIPOLE;
break;
2021-06-15 15:13:47 +02:00
case (IMTQ::CC::GET_CAL_MTM_MEASUREMENT):
2022-01-18 11:41:19 +01:00
*foundLen = IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT;
*foundId = IMTQ::GET_CAL_MTM_MEASUREMENT;
break;
2021-06-15 15:13:47 +02:00
case (IMTQ::CC::GET_RAW_MTM_MEASUREMENT):
2022-01-18 11:41:19 +01:00
*foundLen = IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT;
*foundId = IMTQ::GET_RAW_MTM_MEASUREMENT;
break;
2021-06-15 15:13:47 +02:00
case (IMTQ::CC::SELF_TEST_CMD):
2022-01-18 11:41:19 +01:00
*foundLen = IMTQ::SIZE_STATUS_REPLY;
result = getSelfTestCommandId(foundId);
break;
2021-06-15 15:13:47 +02:00
case (IMTQ::CC::GET_SELF_TEST_RESULT):
2022-01-18 11:41:19 +01:00
*foundLen = IMTQ::SIZE_SELF_TEST_RESULTS;
*foundId = IMTQ::GET_SELF_TEST_RESULT;
break;
2021-06-15 15:13:47 +02:00
default:
2022-01-18 11:41:19 +01:00
sif::debug << "IMTQHandler::scanForReply: Reply contains invalid command code" << std::endl;
result = IGNORE_REPLY_DATA;
break;
}
2021-03-17 11:14:48 +01:00
2022-01-18 11:41:19 +01:00
return result;
2021-03-17 11:14:48 +01:00
}
2022-01-18 11:41:19 +01:00
ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
ReturnValue_t result = RETURN_OK;
2021-03-17 11:14:48 +01:00
2022-01-18 11:41:19 +01:00
result = parseStatusByte(packet);
2021-03-17 11:14:48 +01:00
2022-01-18 11:41:19 +01:00
if (result != RETURN_OK) {
return result;
}
2021-03-17 11:14:48 +01:00
2022-01-18 11:41:19 +01:00
switch (id) {
2021-06-11 11:53:43 +02:00
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):
2021-04-25 15:53:44 +02:00
case (IMTQ::START_ACTUATION_DIPOLE):
2021-04-26 07:56:02 +02:00
case (IMTQ::START_MTM_MEASUREMENT):
2022-01-18 11:41:19 +01:00
/* Replies only the status byte which is already handled with parseStatusByte */
break;
2021-03-17 11:14:48 +01:00
case (IMTQ::GET_ENG_HK_DATA):
2022-01-18 11:41:19 +01:00
fillEngHkDataset(packet);
break;
2021-04-25 15:53:44 +02:00
case (IMTQ::GET_COMMANDED_DIPOLE):
2022-01-18 11:41:19 +01:00
handleGetCommandedDipoleReply(packet);
break;
2021-04-26 07:56:02 +02:00
case (IMTQ::GET_CAL_MTM_MEASUREMENT):
2022-01-18 11:41:19 +01:00
fillCalibratedMtmDataset(packet);
break;
2021-04-26 07:56:02 +02:00
case (IMTQ::GET_RAW_MTM_MEASUREMENT):
2022-01-18 11:41:19 +01:00
fillRawMtmDataset(packet);
break;
2021-06-04 16:45:54 +02:00
case (IMTQ::GET_SELF_TEST_RESULT):
2022-01-18 11:41:19 +01:00
handleSelfTestReply(packet);
break;
2021-03-17 11:14:48 +01:00
default: {
2022-01-18 11:41:19 +01:00
sif::debug << "IMTQHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
2021-06-15 15:13:47 +02:00
}
2022-01-18 11:41:19 +01:00
}
2021-03-17 11:14:48 +01:00
2022-01-18 11:41:19 +01:00
return RETURN_OK;
2021-03-17 11:14:48 +01:00
}
2022-01-18 11:41:19 +01:00
void IMTQHandler::setNormalDatapoolEntriesInvalid() {}
2021-04-25 12:47:41 +02:00
2021-06-12 15:45:24 +02:00
LocalPoolDataSetBase* IMTQHandler::getDataSetHandle(sid_t sid) {
2022-01-18 11:41:19 +01:00
if (sid == engHkDataset.getSid()) {
return &engHkDataset;
} else if (sid == calMtmMeasurementSet.getSid()) {
return &calMtmMeasurementSet;
} else if (sid == rawMtmMeasurementSet.getSid()) {
return &rawMtmMeasurementSet;
} else if (sid == posXselfTestDataset.getSid()) {
return &posXselfTestDataset;
} else if (sid == negXselfTestDataset.getSid()) {
return &negXselfTestDataset;
} else if (sid == posYselfTestDataset.getSid()) {
return &posYselfTestDataset;
} else if (sid == negYselfTestDataset.getSid()) {
return &negYselfTestDataset;
} else if (sid == posZselfTestDataset.getSid()) {
return &posZselfTestDataset;
} else if (sid == negZselfTestDataset.getSid()) {
return &negZselfTestDataset;
} else {
sif::error << "IMTQHandler::getDataSetHandle: Invalid sid" << std::endl;
return nullptr;
}
2021-06-12 15:45:24 +02:00
}
2022-01-18 11:41:19 +01:00
uint32_t IMTQHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; }
2021-04-25 12:47:41 +02:00
ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
2022-01-18 11:41:19 +01:00
LocalDataPoolManager& poolManager) {
/** Entries of engineering housekeeping dataset */
localDataPoolMap.emplace(IMTQ::DIGITAL_VOLTAGE_MV, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::ANALOG_VOLTAGE_MV, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::DIGITAL_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::ANALOG_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::COIL_X_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::COIL_Y_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::COIL_Z_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::COIL_X_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry<uint16_t>({0}));
/** Entries of calibrated MTM measurement dataset */
localDataPoolMap.emplace(IMTQ::MTM_CAL_X, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(IMTQ::MTM_CAL_Y, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(IMTQ::MTM_CAL_Z, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));
/** Entries of raw MTM measurement dataset */
localDataPoolMap.emplace(IMTQ::MTM_RAW_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::MTM_RAW_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::MTM_RAW_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry<uint8_t>({0}));
/** INIT measurements for positive X axis test */
localDataPoolMap.emplace(IMTQ::INIT_POS_X_ERR, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_X_RAW_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_X_RAW_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_X_RAW_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_X_CAL_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_X_CAL_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_X_CAL_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_X_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_Y_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_Z_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>({0}));
/** INIT measurements for negative X axis test */
localDataPoolMap.emplace(IMTQ::INIT_NEG_X_ERR, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_X_RAW_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_X_RAW_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_X_RAW_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_X_CAL_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_X_CAL_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_X_CAL_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_X_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_Y_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_Z_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>({0}));
/** INIT measurements for positive Y axis test */
localDataPoolMap.emplace(IMTQ::INIT_POS_Y_ERR, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Y_RAW_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Y_RAW_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Y_RAW_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Y_CAL_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Y_CAL_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Y_CAL_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_X_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_Y_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_Z_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>({0}));
/** INIT measurements for negative Y axis test */
localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_ERR, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_RAW_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_RAW_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_RAW_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_CAL_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_CAL_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_CAL_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_X_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_Y_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_Z_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>({0}));
/** INIT measurements for positive Z axis test */
localDataPoolMap.emplace(IMTQ::INIT_POS_Z_ERR, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Z_RAW_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Z_RAW_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Z_RAW_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Z_CAL_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Z_CAL_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Z_CAL_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_X_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_Y_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_Z_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>({0}));
/** INIT measurements for negative Z axis test */
localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_ERR, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_RAW_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_RAW_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_RAW_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_CAL_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_CAL_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_CAL_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_X_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_Y_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_Z_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_X_ERR, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_X_RAW_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::POS_X_RAW_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::POS_X_RAW_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::POS_X_CAL_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::POS_X_CAL_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::POS_X_CAL_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::POS_X_COIL_X_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::POS_X_COIL_Y_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::POS_X_COIL_Z_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::POS_X_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_X_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_X_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_X_ERR, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_X_RAW_MAG_X, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_X_RAW_MAG_Y, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_X_RAW_MAG_Z, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_X_CAL_MAG_X, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_X_CAL_MAG_Y, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_X_CAL_MAG_Z, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_X_COIL_X_CURRENT, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_X_COIL_Y_CURRENT, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_X_COIL_Z_CURRENT, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_X_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_X_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_X_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Y_ERR, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Y_RAW_MAG_X, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Y_RAW_MAG_Y, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Y_RAW_MAG_Z, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Y_CAL_MAG_X, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Y_CAL_MAG_Y, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Y_CAL_MAG_Z, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Y_COIL_X_CURRENT, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Y_COIL_Y_CURRENT, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Y_COIL_Z_CURRENT, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Y_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Y_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Y_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Y_ERR, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Y_RAW_MAG_X, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Y_RAW_MAG_Y, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Y_RAW_MAG_Z, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Y_CAL_MAG_X, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Y_CAL_MAG_Y, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Y_CAL_MAG_Z, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_X_CURRENT, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_Y_CURRENT, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_Z_CURRENT, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Z_ERR, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Z_RAW_MAG_X, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Z_RAW_MAG_Y, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Z_RAW_MAG_Z, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Z_CAL_MAG_X, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Z_CAL_MAG_Y, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Z_CAL_MAG_Z, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Z_COIL_X_CURRENT, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Z_COIL_Y_CURRENT, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Z_COIL_Z_CURRENT, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Z_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Z_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::POS_Z_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Z_ERR, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Z_RAW_MAG_X, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Z_RAW_MAG_Y, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Z_RAW_MAG_Z, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Z_CAL_MAG_X, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Z_CAL_MAG_Y, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Z_CAL_MAG_Z, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_X_CURRENT, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_Y_CURRENT, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_Z_CURRENT, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>({0}));
/** FINA measurements for positive X axis test */
localDataPoolMap.emplace(IMTQ::FINA_POS_X_ERR, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_X_RAW_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_X_RAW_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_X_RAW_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_X_CAL_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_X_CAL_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_X_CAL_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_X_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Y_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Z_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>({0}));
/** FINA measurements for negative X axis test */
localDataPoolMap.emplace(IMTQ::FINA_NEG_X_ERR, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_X_RAW_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_X_RAW_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_X_RAW_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_X_CAL_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_X_CAL_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_X_CAL_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_X_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_Y_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_Z_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>({0}));
/** FINA measurements for positive Y axis test */
localDataPoolMap.emplace(IMTQ::FINA_POS_Y_ERR, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Y_RAW_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Y_RAW_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Y_RAW_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Y_CAL_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Y_CAL_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Y_CAL_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_X_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_Y_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_Z_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>({0}));
/** FINA measurements for negative Y axis test */
localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_ERR, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_RAW_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_RAW_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_RAW_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_CAL_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_CAL_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_CAL_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_X_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_Y_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_Z_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>({0}));
/** FINA measurements for positive Z axis test */
localDataPoolMap.emplace(IMTQ::FINA_POS_Z_ERR, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Z_RAW_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Z_RAW_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Z_RAW_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Z_CAL_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Z_CAL_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Z_CAL_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_X_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_Y_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_Z_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>({0}));
/** FINA measurements for negative Z axis test */
localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_ERR, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_RAW_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_RAW_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_RAW_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_CAL_MAG_X, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_CAL_MAG_Y, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_CAL_MAG_Z, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_X_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_Y_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_Z_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>({0}));
poolManager.subscribeForPeriodicPacket(engHkDataset.getSid(), false, 10.0, true);
poolManager.subscribeForPeriodicPacket(calMtmMeasurementSet.getSid(), false, 10.0, true);
2022-01-18 11:41:19 +01:00
return HasReturnvaluesIF::RETURN_OK;
2021-04-25 12:47:41 +02:00
}
2021-06-11 11:53:43 +02:00
ReturnValue_t IMTQHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
2022-01-18 11:41:19 +01:00
DeviceCommandId_t commandId = getPendingCommand();
switch (commandId) {
2021-06-15 15:13:47 +02:00
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:
2022-01-18 11:41:19 +01:00
*id = commandId;
break;
2021-06-15 15:13:47 +02:00
default:
2022-01-18 11:41:19 +01:00
sif::error << "IMTQHandler::getSelfTestCommandId: Reply does not match to pending "
<< "command" << std::endl;
return UNEXPECTED_SELF_TEST_REPLY;
}
return RETURN_OK;
2021-06-11 11:53:43 +02:00
}
2021-03-17 11:14:48 +01:00
ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) {
2022-01-18 11:41:19 +01:00
uint8_t cmdErrorField = *(packet + 1) & 0xF;
switch (cmdErrorField) {
2021-03-17 11:14:48 +01:00
case 0:
2022-01-18 11:41:19 +01:00
return RETURN_OK;
2021-03-17 11:14:48 +01:00
case 1:
2022-01-18 11:41:19 +01:00
sif::error << "IMTQHandler::parseStatusByte: Command rejected without reason" << std::endl;
return REJECTED_WITHOUT_REASON;
2021-03-17 11:14:48 +01:00
case 2:
2022-01-18 11:41:19 +01:00
sif::error << "IMTQHandler::parseStatusByte: Command has invalid command code" << std::endl;
return INVALID_COMMAND_CODE;
2021-03-17 11:14:48 +01:00
case 3:
2022-01-18 11:41:19 +01:00
sif::error << "IMTQHandler::parseStatusByte: Command has missing parameter" << std::endl;
return PARAMETER_MISSING;
2021-03-17 11:14:48 +01:00
case 4:
2022-01-18 11:41:19 +01:00
sif::error << "IMTQHandler::parseStatusByte: Command has invalid parameter" << std::endl;
return PARAMETER_INVALID;
2021-03-17 11:14:48 +01:00
case 5:
2022-01-18 11:41:19 +01:00
sif::error << "IMTQHandler::parseStatusByte: CC unavailable" << std::endl;
return CC_UNAVAILABLE;
2021-03-17 11:14:48 +01:00
case 7:
2022-01-18 11:41:19 +01:00
sif::error << "IMTQHandler::parseStatusByte: IMQT replied internal processing error"
<< std::endl;
return INTERNAL_PROCESSING_ERROR;
2021-03-17 11:14:48 +01:00
default:
2022-01-18 11:41:19 +01:00
sif::error << "IMTQHandler::parseStatusByte: CMD Error field contains unknown error code "
<< cmdErrorField << std::endl;
return CMD_ERR_UNKNOWN;
}
2021-03-17 11:14:48 +01:00
}
void IMTQHandler::fillEngHkDataset(const uint8_t* packet) {
2022-01-18 11:41:19 +01:00
PoolReadGuard rg(&engHkDataset);
uint8_t offset = 2;
engHkDataset.digitalVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
engHkDataset.analogVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
engHkDataset.digitalCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
engHkDataset.analogCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
engHkDataset.coilXCurrentmA =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
engHkDataset.coilYCurrentmA =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
engHkDataset.coilZCurrentmA =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
engHkDataset.coilXTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
offset += 2;
engHkDataset.coilYTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
offset += 2;
engHkDataset.coilZTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
offset += 2;
engHkDataset.mcuTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_IMTQ == 1
sif::info << "IMTQ digital voltage: " << engHkDataset.digitalVoltageMv << " mV" << std::endl;
sif::info << "IMTQ analog voltage: " << engHkDataset.analogVoltageMv << " mV" << std::endl;
sif::info << "IMTQ digital current: " << engHkDataset.digitalCurrentmA << " mA" << std::endl;
sif::info << "IMTQ analog current: " << engHkDataset.analogCurrentmA << " mA" << std::endl;
sif::info << "IMTQ coil X current: " << engHkDataset.coilXCurrentmA << " mA" << std::endl;
sif::info << "IMTQ coil Y current: " << engHkDataset.coilYCurrentmA << " mA" << std::endl;
sif::info << "IMTQ coil Z current: " << engHkDataset.coilZCurrentmA << " mA" << std::endl;
sif::info << "IMTQ coil X temperature: " << engHkDataset.coilXTemperature << " °C" << std::endl;
sif::info << "IMTQ coil Y temperature: " << engHkDataset.coilYTemperature << " °C" << std::endl;
sif::info << "IMTQ coil Z temperature: " << engHkDataset.coilZTemperature << " °C" << std::endl;
sif::info << "IMTQ coil MCU temperature: " << engHkDataset.mcuTemperature << " °C" << std::endl;
2021-03-17 11:14:48 +01:00
#endif
}
2022-01-27 16:04:45 +01:00
void IMTQHandler::setToGoToNormal(bool enable) { this->goToNormalMode = enable; }
2021-03-17 11:14:48 +01:00
2021-04-25 15:53:44 +02:00
void IMTQHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) {
2022-01-18 11:41:19 +01:00
if (wiretappingMode == RAW) {
/* Data already sent in doGetRead() */
return;
}
DeviceReplyMap::iterator iter = deviceReplyMap.find(replyId);
if (iter == deviceReplyMap.end()) {
sif::debug << "IMTQHandler::handleDeviceTM: Unknown reply id" << std::endl;
return;
}
MessageQueueId_t queueId = iter->second.command->second.sendReplyTo;
if (queueId == NO_COMMANDER) {
return;
}
ReturnValue_t result = actionHelper.reportData(queueId, replyId, data, dataSize);
if (result != RETURN_OK) {
sif::debug << "IMTQHandler::handleDeviceTM: Failed to report data" << std::endl;
return;
}
2021-04-25 15:53:44 +02:00
}
2021-04-26 07:56:02 +02:00
void IMTQHandler::handleGetCommandedDipoleReply(const uint8_t* packet) {
2022-01-18 11:41:19 +01:00
uint8_t tmData[6];
/* Switching endianess of received dipole values */
tmData[0] = *(packet + 3);
tmData[1] = *(packet + 2);
tmData[2] = *(packet + 5);
tmData[3] = *(packet + 4);
tmData[4] = *(packet + 7);
tmData[5] = *(packet + 6);
handleDeviceTM(tmData, sizeof(tmData), IMTQ::GET_COMMANDED_DIPOLE);
2021-04-26 07:56:02 +02:00
}
void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
2022-01-18 11:41:19 +01:00
PoolReadGuard rg(&calMtmMeasurementSet);
int8_t offset = 2;
calMtmMeasurementSet.mtmXnT = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
calMtmMeasurementSet.mtmYnT = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
calMtmMeasurementSet.mtmZnT = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
calMtmMeasurementSet.coilActuationStatus = (*(packet + offset + 3) << 24) |
(*(packet + offset + 2) << 16) |
(*(packet + offset + 1) << 8) | (*(packet + offset));
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_IMTQ == 1
sif::info << "IMTQ calibrated MTM measurement X: " << calMtmMeasurementSet.mtmXnT << " nT"
2021-04-26 07:56:02 +02:00
<< std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ calibrated MTM measurement Y: " << calMtmMeasurementSet.mtmYnT << " nT"
2021-04-26 07:56:02 +02:00
<< std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ calibrated MTM measurement Z: " << calMtmMeasurementSet.mtmZnT << " nT"
2021-04-26 07:56:02 +02:00
<< std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ coil actuation status during MTM measurement: "
<< (unsigned int)calMtmMeasurementSet.coilActuationStatus.value << std::endl;
2021-04-26 07:56:02 +02:00
#endif
}
void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) {
2022-01-18 11:41:19 +01:00
PoolReadGuard rg(&rawMtmMeasurementSet);
int8_t offset = 2;
rawMtmMeasurementSet.mtmXnT = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset)) *
7.5;
offset += 4;
rawMtmMeasurementSet.mtmYnT = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset)) *
7.5;
offset += 4;
rawMtmMeasurementSet.mtmZnT = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset)) *
7.5;
offset += 4;
rawMtmMeasurementSet.coilActuationStatus = (*(packet + offset + 3) << 24) |
(*(packet + offset + 2) << 16) |
(*(packet + offset + 1) << 8) | (*(packet + offset));
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_IMTQ == 1
sif::info << "IMTQ raw MTM measurement X: " << rawMtmMeasurementSet.mtmXnT << " nT" << std::endl;
sif::info << "IMTQ raw MTM measurement Y: " << rawMtmMeasurementSet.mtmYnT << " nT" << std::endl;
sif::info << "IMTQ raw MTM measurement Z: " << rawMtmMeasurementSet.mtmZnT << " nT" << std::endl;
sif::info << "IMTQ coil actuation status during MTM measurement: "
<< (unsigned int)rawMtmMeasurementSet.coilActuationStatus.value << std::endl;
2021-04-26 07:56:02 +02:00
#endif
}
2021-06-04 16:45:54 +02:00
void IMTQHandler::handleSelfTestReply(const uint8_t* packet) {
2022-01-18 11:41:19 +01:00
uint16_t offset = 2;
checkErrorByte(*(packet + offset), *(packet + offset + 1));
2021-06-04 16:45:54 +02:00
2022-01-18 11:41:19 +01:00
switch (*(packet + IMTQ::MAIN_STEP_OFFSET)) {
2021-06-11 11:53:43 +02:00
case IMTQ::SELF_TEST_STEPS::X_POSITIVE: {
2022-01-18 11:41:19 +01:00
handlePositiveXSelfTestReply(packet);
break;
2021-06-11 11:53:43 +02:00
}
2021-06-15 15:13:47 +02:00
case IMTQ::SELF_TEST_STEPS::X_NEGATIVE: {
2022-01-18 11:41:19 +01:00
handleNegativeXSelfTestReply(packet);
break;
2021-06-15 15:13:47 +02:00
}
case IMTQ::SELF_TEST_STEPS::Y_POSITIVE: {
2022-01-18 11:41:19 +01:00
handlePositiveYSelfTestReply(packet);
break;
2021-06-15 15:13:47 +02:00
}
case IMTQ::SELF_TEST_STEPS::Y_NEGATIVE: {
2022-01-18 11:41:19 +01:00
handleNegativeYSelfTestReply(packet);
break;
2021-06-15 15:13:47 +02:00
}
case IMTQ::SELF_TEST_STEPS::Z_POSITIVE: {
2022-01-18 11:41:19 +01:00
handlePositiveZSelfTestReply(packet);
break;
2021-06-15 15:13:47 +02:00
}
case IMTQ::SELF_TEST_STEPS::Z_NEGATIVE: {
2022-01-18 11:41:19 +01:00
handleNegativeZSelfTestReply(packet);
break;
2021-06-15 15:13:47 +02:00
}
2021-06-11 11:53:43 +02:00
default:
2022-01-18 11:41:19 +01:00
break;
}
2021-06-11 11:53:43 +02:00
}
2021-06-04 16:45:54 +02:00
2021-06-11 11:53:43 +02:00
void IMTQHandler::handlePositiveXSelfTestReply(const uint8_t* packet) {
2022-01-18 11:41:19 +01:00
PoolReadGuard rg(&posXselfTestDataset);
uint16_t offset = 2;
/** Init measurements */
posXselfTestDataset.initErr = *(packet + offset);
offset += 2; // STEP byte will not be stored
posXselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset)) *
7.5;
offset += 4;
posXselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset)) *
7.5;
offset += 4;
posXselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset)) *
7.5;
offset += 4;
posXselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
posXselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
posXselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
posXselfTestDataset.initCoilXCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
posXselfTestDataset.initCoilYCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
posXselfTestDataset.initCoilZCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
posXselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
posXselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
posXselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
/** +X measurements */
checkErrorByte(*(packet + offset), *(packet + offset + 1));
posXselfTestDataset.err = *(packet + offset);
offset += 2; // STEP byte will not be stored
posXselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset)) *
7.5;
offset += 4;
posXselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset)) *
7.5;
offset += 4;
posXselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset)) *
7.5;
offset += 4;
posXselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
posXselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
posXselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
posXselfTestDataset.coilXCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
posXselfTestDataset.coilYCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
posXselfTestDataset.coilZCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
posXselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
posXselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
posXselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
/** FINA measurements */
checkErrorByte(*(packet + offset), *(packet + offset + 1));
posXselfTestDataset.finaErr = *(packet + offset);
offset += 2; // STEP byte will not be stored
posXselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset)) *
7.5;
offset += 4;
posXselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset)) *
7.5;
offset += 4;
posXselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset)) *
7.5;
offset += 4;
posXselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
posXselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
posXselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
posXselfTestDataset.finaCoilXCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
posXselfTestDataset.finaCoilYCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
posXselfTestDataset.finaCoilZCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
posXselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
posXselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
posXselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_IMTQ == 1
sif::info << "IMTQ self test (INIT) err: "
2021-06-15 15:13:47 +02:00
<< static_cast<unsigned int>(posXselfTestDataset.initErr.value) << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) raw magnetic field X: " << posXselfTestDataset.initRawMagX
2021-06-12 15:45:24 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << posXselfTestDataset.initRawMagY
2021-06-12 15:45:24 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << posXselfTestDataset.initRawMagZ
2021-06-12 15:45:24 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) calibrated magnetic field X: "
2021-06-15 15:13:47 +02:00
<< posXselfTestDataset.initCalMagX << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: "
2021-06-15 15:13:47 +02:00
<< posXselfTestDataset.initCalMagY << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: "
2021-06-15 15:13:47 +02:00
<< posXselfTestDataset.initCalMagZ << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil X current: " << posXselfTestDataset.initCoilXCurrent
2021-06-12 15:45:24 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Y current: " << posXselfTestDataset.initCoilYCurrent
2021-06-12 15:45:24 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Z current: " << posXselfTestDataset.initCoilZCurrent
2021-06-12 15:45:24 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil X temperature: "
2021-06-11 11:53:43 +02:00
<< posXselfTestDataset.initCoilXTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Y temperature: "
2021-06-11 11:53:43 +02:00
<< posXselfTestDataset.initCoilYTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Z temperature: "
2021-06-11 11:53:43 +02:00
<< posXselfTestDataset.initCoilZTemperature << " °C" << std::endl;
2021-06-04 16:45:54 +02:00
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+X) err: "
2021-06-15 15:13:47 +02:00
<< static_cast<unsigned int>(posXselfTestDataset.err.value) << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+X) raw magnetic field X: " << posXselfTestDataset.rawMagX << " nT"
<< std::endl;
sif::info << "IMTQ self test (+X) raw magnetic field Y: " << posXselfTestDataset.rawMagY << " nT"
<< std::endl;
sif::info << "IMTQ self test (+X) raw magnetic field Z: " << posXselfTestDataset.rawMagZ << " nT"
<< std::endl;
sif::info << "IMTQ self test (+X) calibrated magnetic field X: " << posXselfTestDataset.calMagX
2021-06-12 15:45:24 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+X) calibrated magnetic field Y: " << posXselfTestDataset.calMagY
2021-06-12 15:45:24 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+X) calibrated magnetic field Z: " << posXselfTestDataset.calMagZ
2021-06-12 15:45:24 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+X) coil X current: " << posXselfTestDataset.coilXCurrent << " mA"
2021-06-15 15:13:47 +02:00
<< std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+X) coil Y current: " << posXselfTestDataset.coilYCurrent << " mA"
2021-06-15 15:13:47 +02:00
<< std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+X) coil Z current: " << posXselfTestDataset.coilZCurrent << " mA"
2021-06-15 15:13:47 +02:00
<< std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+X) coil X temperature: " << posXselfTestDataset.coilXTemperature
2021-06-15 15:13:47 +02:00
<< " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+X) coil Y temperature: " << posXselfTestDataset.coilYTemperature
2021-06-15 15:13:47 +02:00
<< " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+X) coil Z temperature: " << posXselfTestDataset.coilZTemperature
2021-06-15 15:13:47 +02:00
<< " °C" << std::endl;
2021-06-04 16:45:54 +02:00
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) err: "
2021-06-11 11:53:43 +02:00
<< static_cast<unsigned int>(posXselfTestDataset.finaErr.value) << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) raw magnetic field X: " << posXselfTestDataset.finaRawMagX
2021-06-12 15:45:24 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << posXselfTestDataset.finaRawMagY
2021-06-12 15:45:24 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << posXselfTestDataset.finaRawMagZ
2021-06-12 15:45:24 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) calibrated magnetic field X: "
2021-06-15 15:13:47 +02:00
<< posXselfTestDataset.finaCalMagX << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: "
2021-06-15 15:13:47 +02:00
<< posXselfTestDataset.finaCalMagY << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: "
2021-06-15 15:13:47 +02:00
<< posXselfTestDataset.finaCalMagZ << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil X current: " << posXselfTestDataset.finaCoilXCurrent
2021-06-12 15:45:24 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Y current: " << posXselfTestDataset.finaCoilYCurrent
2021-06-12 15:45:24 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Z current: " << posXselfTestDataset.finaCoilZCurrent
2021-06-12 15:45:24 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil X temperature: "
2021-06-11 11:53:43 +02:00
<< posXselfTestDataset.finaCoilXTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Y temperature: "
2021-06-11 11:53:43 +02:00
<< posXselfTestDataset.finaCoilYTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Z temperature: "
2021-06-11 11:53:43 +02:00
<< posXselfTestDataset.finaCoilZTemperature << " °C" << std::endl;
2021-06-04 16:45:54 +02:00
#endif
}
2021-06-15 15:13:47 +02:00
void IMTQHandler::handleNegativeXSelfTestReply(const uint8_t* packet) {
2022-01-18 11:41:19 +01:00
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<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
negXselfTestDataset.initCoilYCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
negXselfTestDataset.initCoilZCurrent =
static_cast<int16_t>(*(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<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
negXselfTestDataset.coilYCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
negXselfTestDataset.coilZCurrent =
static_cast<int16_t>(*(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<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
negXselfTestDataset.finaCoilYCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
negXselfTestDataset.finaCoilZCurrent =
static_cast<int16_t>(*(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 && OBSW_DEBUG_IMTQ == 1
sif::info << "IMTQ self test (INIT) err: "
2021-06-15 15:13:47 +02:00
<< static_cast<unsigned int>(negXselfTestDataset.initErr.value) << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) raw magnetic field X: " << negXselfTestDataset.initRawMagX
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << negXselfTestDataset.initRawMagY
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << negXselfTestDataset.initRawMagZ
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) calibrated magnetic field X: "
2021-06-15 15:13:47 +02:00
<< negXselfTestDataset.initCalMagX << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: "
2021-06-15 15:13:47 +02:00
<< negXselfTestDataset.initCalMagY << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: "
2021-06-15 15:13:47 +02:00
<< negXselfTestDataset.initCalMagZ << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil X current: " << negXselfTestDataset.initCoilXCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Y current: " << negXselfTestDataset.initCoilYCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Z current: " << negXselfTestDataset.initCoilZCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil X temperature: "
2021-06-15 15:13:47 +02:00
<< negXselfTestDataset.initCoilXTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Y temperature: "
2021-06-15 15:13:47 +02:00
<< negXselfTestDataset.initCoilYTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Z temperature: "
2021-06-15 15:13:47 +02:00
<< negXselfTestDataset.initCoilZTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-X) err: "
2021-06-15 15:13:47 +02:00
<< static_cast<unsigned int>(negXselfTestDataset.err.value) << std::endl;
2022-01-18 11:41:19 +01:00
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
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-X) calibrated magnetic field Y: " << negXselfTestDataset.calMagY
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-X) calibrated magnetic field Z: " << negXselfTestDataset.calMagZ
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-X) coil X current: " << negXselfTestDataset.coilXCurrent << " mA"
2021-06-15 15:13:47 +02:00
<< std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-X) coil Y current: " << negXselfTestDataset.coilYCurrent << " mA"
2021-06-15 15:13:47 +02:00
<< std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-X) coil Z current: " << negXselfTestDataset.coilZCurrent << " mA"
2021-06-15 15:13:47 +02:00
<< std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-X) coil X temperature: " << negXselfTestDataset.coilXTemperature
2021-06-15 15:13:47 +02:00
<< " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-X) coil Y temperature: " << negXselfTestDataset.coilYTemperature
2021-06-15 15:13:47 +02:00
<< " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-X) coil Z temperature: " << negXselfTestDataset.coilZTemperature
2021-06-15 15:13:47 +02:00
<< " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) err: "
2021-06-15 15:13:47 +02:00
<< static_cast<unsigned int>(negXselfTestDataset.finaErr.value) << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) raw magnetic field X: " << negXselfTestDataset.finaRawMagX
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << negXselfTestDataset.finaRawMagY
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << negXselfTestDataset.finaRawMagZ
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) calibrated magnetic field X: "
2021-06-15 15:13:47 +02:00
<< negXselfTestDataset.finaCalMagX << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: "
2021-06-15 15:13:47 +02:00
<< negXselfTestDataset.finaCalMagY << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: "
2021-06-15 15:13:47 +02:00
<< negXselfTestDataset.finaCalMagZ << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil X current: " << negXselfTestDataset.finaCoilXCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Y current: " << negXselfTestDataset.finaCoilYCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Z current: " << negXselfTestDataset.finaCoilZCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil X temperature: "
2021-06-15 15:13:47 +02:00
<< negXselfTestDataset.finaCoilXTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Y temperature: "
2021-06-15 15:13:47 +02:00
<< negXselfTestDataset.finaCoilYTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Z temperature: "
2021-06-15 15:13:47 +02:00
<< negXselfTestDataset.finaCoilZTemperature << " °C" << std::endl;
#endif
}
void IMTQHandler::handlePositiveYSelfTestReply(const uint8_t* packet) {
2022-01-18 11:41:19 +01:00
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<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
posYselfTestDataset.initCoilYCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
posYselfTestDataset.initCoilZCurrent =
static_cast<int16_t>(*(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<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
posYselfTestDataset.coilYCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
posYselfTestDataset.coilZCurrent =
static_cast<int16_t>(*(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<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
posYselfTestDataset.finaCoilYCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
posYselfTestDataset.finaCoilZCurrent =
static_cast<int16_t>(*(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 && OBSW_DEBUG_IMTQ == 1
sif::info << "IMTQ self test (INIT) err: "
2021-06-15 15:13:47 +02:00
<< static_cast<unsigned int>(posYselfTestDataset.initErr.value) << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) raw magnetic field X: " << posYselfTestDataset.initRawMagX
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << posYselfTestDataset.initRawMagY
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << posYselfTestDataset.initRawMagZ
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) calibrated magnetic field X: "
2021-06-15 15:13:47 +02:00
<< posYselfTestDataset.initCalMagX << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: "
2021-06-15 15:13:47 +02:00
<< posYselfTestDataset.initCalMagY << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: "
2021-06-15 15:13:47 +02:00
<< posYselfTestDataset.initCalMagZ << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil X current: " << posYselfTestDataset.initCoilXCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Y current: " << posYselfTestDataset.initCoilYCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Z current: " << posYselfTestDataset.initCoilZCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil X temperature: "
2021-06-15 15:13:47 +02:00
<< posYselfTestDataset.initCoilXTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Y temperature: "
2021-06-15 15:13:47 +02:00
<< posYselfTestDataset.initCoilYTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Z temperature: "
2021-06-15 15:13:47 +02:00
<< posYselfTestDataset.initCoilZTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+Y) err: "
2021-06-15 15:13:47 +02:00
<< static_cast<unsigned int>(posYselfTestDataset.err.value) << std::endl;
2022-01-18 11:41:19 +01:00
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
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+Y) calibrated magnetic field Y: " << posYselfTestDataset.calMagY
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+Y) calibrated magnetic field Z: " << posYselfTestDataset.calMagZ
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+Y) coil X current: " << posYselfTestDataset.coilXCurrent << " mA"
2021-06-15 15:13:47 +02:00
<< std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+Y) coil Y current: " << posYselfTestDataset.coilYCurrent << " mA"
2021-06-15 15:13:47 +02:00
<< std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+Y) coil Z current: " << posYselfTestDataset.coilZCurrent << " mA"
2021-06-15 15:13:47 +02:00
<< std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+Y) coil X temperature: " << posYselfTestDataset.coilXTemperature
2021-06-15 15:13:47 +02:00
<< " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+Y) coil Y temperature: " << posYselfTestDataset.coilYTemperature
2021-06-15 15:13:47 +02:00
<< " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+Y) coil Z temperature: " << posYselfTestDataset.coilZTemperature
2021-06-15 15:13:47 +02:00
<< " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) err: "
2021-06-15 15:13:47 +02:00
<< static_cast<unsigned int>(posYselfTestDataset.finaErr.value) << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) raw magnetic field X: " << posYselfTestDataset.finaRawMagX
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << posYselfTestDataset.finaRawMagY
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << posYselfTestDataset.finaRawMagZ
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) calibrated magnetic field X: "
2021-06-15 15:13:47 +02:00
<< posYselfTestDataset.finaCalMagX << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: "
2021-06-15 15:13:47 +02:00
<< posYselfTestDataset.finaCalMagY << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: "
2021-06-15 15:13:47 +02:00
<< posYselfTestDataset.finaCalMagZ << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil X current: " << posYselfTestDataset.finaCoilXCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Y current: " << posYselfTestDataset.finaCoilYCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Z current: " << posYselfTestDataset.finaCoilZCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil X temperature: "
2021-06-15 15:13:47 +02:00
<< posYselfTestDataset.finaCoilXTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Y temperature: "
2021-06-15 15:13:47 +02:00
<< posYselfTestDataset.finaCoilYTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Z temperature: "
2021-06-15 15:13:47 +02:00
<< posYselfTestDataset.finaCoilZTemperature << " °C" << std::endl;
#endif
}
void IMTQHandler::handleNegativeYSelfTestReply(const uint8_t* packet) {
2022-01-18 11:41:19 +01:00
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<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
negYselfTestDataset.initCoilYCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
negYselfTestDataset.initCoilZCurrent =
static_cast<int16_t>(*(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<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
negYselfTestDataset.coilYCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
negYselfTestDataset.coilZCurrent =
static_cast<int16_t>(*(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<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
negYselfTestDataset.finaCoilYCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
negYselfTestDataset.finaCoilZCurrent =
static_cast<int16_t>(*(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 && OBSW_DEBUG_IMTQ == 1
sif::info << "IMTQ self test (INIT) err: "
2021-06-15 15:13:47 +02:00
<< static_cast<unsigned int>(negYselfTestDataset.initErr.value) << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) raw magnetic field X: " << negYselfTestDataset.initRawMagX
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << negYselfTestDataset.initRawMagY
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << negYselfTestDataset.initRawMagZ
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) calibrated magnetic field X: "
2021-06-15 15:13:47 +02:00
<< negYselfTestDataset.initCalMagX << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: "
2021-06-15 15:13:47 +02:00
<< negYselfTestDataset.initCalMagY << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: "
2021-06-15 15:13:47 +02:00
<< negYselfTestDataset.initCalMagZ << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil X current: " << negYselfTestDataset.initCoilXCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Y current: " << negYselfTestDataset.initCoilYCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Z current: " << negYselfTestDataset.initCoilZCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil X temperature: "
2021-06-15 15:13:47 +02:00
<< negYselfTestDataset.initCoilXTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Y temperature: "
2021-06-15 15:13:47 +02:00
<< negYselfTestDataset.initCoilYTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Z temperature: "
2021-06-15 15:13:47 +02:00
<< negYselfTestDataset.initCoilZTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-Y) err: "
2021-06-15 15:13:47 +02:00
<< static_cast<unsigned int>(negYselfTestDataset.err.value) << std::endl;
2022-01-18 11:41:19 +01:00
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
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-Y) calibrated magnetic field Y: " << negYselfTestDataset.calMagY
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-Y) calibrated magnetic field Z: " << negYselfTestDataset.calMagZ
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-Y) coil X current: " << negYselfTestDataset.coilXCurrent << " mA"
2021-06-15 15:13:47 +02:00
<< std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-Y) coil Y current: " << negYselfTestDataset.coilYCurrent << " mA"
2021-06-15 15:13:47 +02:00
<< std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-Y) coil Z current: " << negYselfTestDataset.coilZCurrent << " mA"
2021-06-15 15:13:47 +02:00
<< std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-Y) coil X temperature: " << negYselfTestDataset.coilXTemperature
2021-06-15 15:13:47 +02:00
<< " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-Y) coil Y temperature: " << negYselfTestDataset.coilYTemperature
2021-06-15 15:13:47 +02:00
<< " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-Y) coil Z temperature: " << negYselfTestDataset.coilZTemperature
2021-06-15 15:13:47 +02:00
<< " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) err: "
2021-06-15 15:13:47 +02:00
<< static_cast<unsigned int>(negYselfTestDataset.finaErr.value) << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) raw magnetic field X: " << negYselfTestDataset.finaRawMagX
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << negYselfTestDataset.finaRawMagY
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << negYselfTestDataset.finaRawMagZ
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) calibrated magnetic field X: "
2021-06-15 15:13:47 +02:00
<< negYselfTestDataset.finaCalMagX << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: "
2021-06-15 15:13:47 +02:00
<< negYselfTestDataset.finaCalMagY << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: "
2021-06-15 15:13:47 +02:00
<< negYselfTestDataset.finaCalMagZ << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil X current: " << negYselfTestDataset.finaCoilXCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Y current: " << negYselfTestDataset.finaCoilYCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Z current: " << negYselfTestDataset.finaCoilZCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil X temperature: "
2021-06-15 15:13:47 +02:00
<< negYselfTestDataset.finaCoilXTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Y temperature: "
2021-06-15 15:13:47 +02:00
<< negYselfTestDataset.finaCoilYTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Z temperature: "
2021-06-15 15:13:47 +02:00
<< negYselfTestDataset.finaCoilZTemperature << " °C" << std::endl;
#endif
}
void IMTQHandler::handlePositiveZSelfTestReply(const uint8_t* packet) {
2022-01-18 11:41:19 +01:00
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<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
posZselfTestDataset.initCoilYCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
posZselfTestDataset.initCoilZCurrent =
static_cast<int16_t>(*(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<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
posZselfTestDataset.coilYCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
posZselfTestDataset.coilZCurrent =
static_cast<int16_t>(*(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<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
posZselfTestDataset.finaCoilYCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
posZselfTestDataset.finaCoilZCurrent =
static_cast<int16_t>(*(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 && OBSW_DEBUG_IMTQ == 1
sif::info << "IMTQ self test (INIT) err: "
2021-06-15 15:13:47 +02:00
<< static_cast<unsigned int>(posZselfTestDataset.initErr.value) << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) raw magnetic field X: " << posZselfTestDataset.initRawMagX
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << posZselfTestDataset.initRawMagY
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << posZselfTestDataset.initRawMagZ
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) calibrated magnetic field X: "
2021-06-15 15:13:47 +02:00
<< posZselfTestDataset.initCalMagX << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: "
2021-06-15 15:13:47 +02:00
<< posZselfTestDataset.initCalMagY << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: "
2021-06-15 15:13:47 +02:00
<< posZselfTestDataset.initCalMagZ << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil X current: " << posZselfTestDataset.initCoilXCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Y current: " << posZselfTestDataset.initCoilYCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Z current: " << posZselfTestDataset.initCoilZCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil X temperature: "
2021-06-15 15:13:47 +02:00
<< posZselfTestDataset.initCoilXTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Y temperature: "
2021-06-15 15:13:47 +02:00
<< posZselfTestDataset.initCoilYTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Z temperature: "
2021-06-15 15:13:47 +02:00
<< posZselfTestDataset.initCoilZTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+Z) err: "
2021-06-15 15:13:47 +02:00
<< static_cast<unsigned int>(posZselfTestDataset.err.value) << std::endl;
2022-01-18 11:41:19 +01:00
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
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+Z) calibrated magnetic field Y: " << posZselfTestDataset.calMagY
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+Z) calibrated magnetic field Z: " << posZselfTestDataset.calMagZ
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+Z) coil X current: " << posZselfTestDataset.coilXCurrent << " mA"
2021-06-15 15:13:47 +02:00
<< std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+Z) coil Y current: " << posZselfTestDataset.coilYCurrent << " mA"
2021-06-15 15:13:47 +02:00
<< std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+Z) coil Z current: " << posZselfTestDataset.coilZCurrent << " mA"
2021-06-15 15:13:47 +02:00
<< std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+Z) coil X temperature: " << posZselfTestDataset.coilXTemperature
2021-06-15 15:13:47 +02:00
<< " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+Z) coil Y temperature: " << posZselfTestDataset.coilYTemperature
2021-06-15 15:13:47 +02:00
<< " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (+Z) coil Z temperature: " << negYselfTestDataset.coilZTemperature
2021-06-15 15:13:47 +02:00
<< " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) err: "
2021-06-15 15:13:47 +02:00
<< static_cast<unsigned int>(posZselfTestDataset.finaErr.value) << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) raw magnetic field X: " << posZselfTestDataset.finaRawMagX
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << posZselfTestDataset.finaRawMagY
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << posZselfTestDataset.finaRawMagZ
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) calibrated magnetic field X: "
2021-06-15 15:13:47 +02:00
<< posZselfTestDataset.finaCalMagX << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: "
2021-06-15 15:13:47 +02:00
<< posZselfTestDataset.finaCalMagY << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: "
2021-06-15 15:13:47 +02:00
<< posZselfTestDataset.finaCalMagZ << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil X current: " << posZselfTestDataset.finaCoilXCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Y current: " << posZselfTestDataset.finaCoilYCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Z current: " << posZselfTestDataset.finaCoilZCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil X temperature: "
2021-06-15 15:13:47 +02:00
<< posZselfTestDataset.finaCoilXTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Y temperature: "
2021-06-15 15:13:47 +02:00
<< posZselfTestDataset.finaCoilYTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Z temperature: "
2021-06-15 15:13:47 +02:00
<< posZselfTestDataset.finaCoilZTemperature << " °C" << std::endl;
#endif
}
void IMTQHandler::handleNegativeZSelfTestReply(const uint8_t* packet) {
2022-01-18 11:41:19 +01:00
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<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
negZselfTestDataset.initCoilYCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
negZselfTestDataset.initCoilZCurrent =
static_cast<int16_t>(*(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<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
negZselfTestDataset.coilYCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
negZselfTestDataset.coilZCurrent =
static_cast<int16_t>(*(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<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
negZselfTestDataset.finaCoilYCurrent =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
negZselfTestDataset.finaCoilZCurrent =
static_cast<int16_t>(*(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 && OBSW_DEBUG_IMTQ == 1
sif::info << "IMTQ self test (INIT) err: "
2021-06-15 15:13:47 +02:00
<< static_cast<unsigned int>(negZselfTestDataset.initErr.value) << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) raw magnetic field X: " << negZselfTestDataset.initRawMagX
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << negZselfTestDataset.initRawMagY
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << negZselfTestDataset.initRawMagZ
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) calibrated magnetic field X: "
2021-06-15 15:13:47 +02:00
<< negZselfTestDataset.initCalMagX << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: "
2021-06-15 15:13:47 +02:00
<< negZselfTestDataset.initCalMagY << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: "
2021-06-15 15:13:47 +02:00
<< negZselfTestDataset.initCalMagZ << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil X current: " << negZselfTestDataset.initCoilXCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Y current: " << negZselfTestDataset.initCoilYCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Z current: " << negZselfTestDataset.initCoilZCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil X temperature: "
2021-06-15 15:13:47 +02:00
<< negZselfTestDataset.initCoilXTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Y temperature: "
2021-06-15 15:13:47 +02:00
<< negZselfTestDataset.initCoilYTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (INIT) coil Z temperature: "
2021-06-15 15:13:47 +02:00
<< negZselfTestDataset.initCoilZTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-Z) err: "
2021-06-15 15:13:47 +02:00
<< static_cast<unsigned int>(negZselfTestDataset.err.value) << std::endl;
2022-01-18 11:41:19 +01:00
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
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-Z) calibrated magnetic field Y: " << negZselfTestDataset.calMagY
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-Z) calibrated magnetic field Z: " << negZselfTestDataset.calMagZ
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-Z) coil X current: " << negZselfTestDataset.coilXCurrent << " mA"
2021-06-15 15:13:47 +02:00
<< std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-Z) coil Y current: " << negZselfTestDataset.coilYCurrent << " mA"
2021-06-15 15:13:47 +02:00
<< std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-Z) coil Z current: " << negZselfTestDataset.coilZCurrent << " mA"
2021-06-15 15:13:47 +02:00
<< std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-Z) coil X temperature: " << negZselfTestDataset.coilXTemperature
2021-06-15 15:13:47 +02:00
<< " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-Z) coil Y temperature: " << negZselfTestDataset.coilYTemperature
2021-06-15 15:13:47 +02:00
<< " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (-Z) coil Z temperature: " << negYselfTestDataset.coilZTemperature
2021-06-15 15:13:47 +02:00
<< " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) err: "
2021-06-15 15:13:47 +02:00
<< static_cast<unsigned int>(negZselfTestDataset.finaErr.value) << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) raw magnetic field X: " << negZselfTestDataset.finaRawMagX
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << negZselfTestDataset.finaRawMagY
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << negZselfTestDataset.finaRawMagZ
2021-06-15 15:13:47 +02:00
<< " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) calibrated magnetic field X: "
2021-06-15 15:13:47 +02:00
<< negZselfTestDataset.finaCalMagX << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: "
2021-06-15 15:13:47 +02:00
<< negZselfTestDataset.finaCalMagY << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: "
2021-06-15 15:13:47 +02:00
<< negZselfTestDataset.finaCalMagZ << " nT" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil X current: " << negZselfTestDataset.finaCoilXCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Y current: " << negZselfTestDataset.finaCoilYCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Z current: " << negZselfTestDataset.finaCoilZCurrent
2021-06-15 15:13:47 +02:00
<< " mA" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil X temperature: "
2021-06-15 15:13:47 +02:00
<< negZselfTestDataset.finaCoilXTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Y temperature: "
2021-06-15 15:13:47 +02:00
<< negZselfTestDataset.finaCoilYTemperature << " °C" << std::endl;
2022-01-18 11:41:19 +01:00
sif::info << "IMTQ self test (FINA) coil Z temperature: "
2021-06-15 15:13:47 +02:00
<< negZselfTestDataset.finaCoilZTemperature << " °C" << std::endl;
#endif
}
void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) {
2022-01-18 11:41:19 +01:00
std::string stepString("");
if (step < 8) {
stepString = makeStepString(step);
} else {
/** This should normally never happen */
sif::debug << "IMTQHandler::checkErrorByte: Invalid step" << std::endl;
return;
}
if (errorByte == 0) {
return;
}
if (errorByte & IMTQ::I2C_FAILURE_MASK) {
triggerEvent(SELF_TEST_I2C_FAILURE, step);
sif::error << "IMTQHandler::checkErrorByte: Self test I2C failure for step " << stepString
<< std::endl;
}
if (errorByte & IMTQ::SPI_FAILURE_MASK) {
triggerEvent(SELF_TEST_SPI_FAILURE, step);
sif::error << "IMTQHandler::checkErrorByte: Self test SPI failure for step " << stepString
<< std::endl;
}
if (errorByte & IMTQ::ADC_FAILURE_MASK) {
triggerEvent(SELF_TEST_ADC_FAILURE, step);
sif::error << "IMTQHandler::checkErrorByte: Self test ADC failure for step " << stepString
<< std::endl;
}
if (errorByte & IMTQ::PWM_FAILURE_MASK) {
triggerEvent(SELF_TEST_PWM_FAILURE, step);
sif::error << "IMTQHandler::checkErrorByte: Self test PWM failure for step " << stepString
<< std::endl;
}
if (errorByte & IMTQ::TC_FAILURE_MASK) {
triggerEvent(SELF_TEST_TC_FAILURE, step);
sif::error << "IMTQHandler::checkErrorByte: Self test TC failure (system failure) for step "
<< stepString << std::endl;
}
if (errorByte & IMTQ::MTM_RANGE_FAILURE_MASK) {
triggerEvent(SELF_TEST_TC_FAILURE, step);
sif::error << "IMTQHandler::checkErrorByte: Self test MTM range failure for step " << stepString
<< std::endl;
}
if (errorByte & IMTQ::COIL_CURRENT_FAILURE_MASK) {
triggerEvent(SELF_TEST_COIL_CURRENT_FAILURE, step);
sif::error << "IMTQHandler::checkErrorByte: Self test coil current outside of expected "
"range for step "
<< stepString << std::endl;
}
if (errorByte & IMTQ::INVALID_ERROR_BYTE) {
triggerEvent(INVALID_ERROR_BYTE, step);
sif::error << "IMTQHandler::checkErrorByte: Self test result of step " << stepString
<< " has invalid error byte" << std::endl;
}
2021-06-15 15:13:47 +02:00
}
std::string IMTQHandler::makeStepString(const uint8_t step) {
2022-01-18 11:41:19 +01:00
std::string stepString("");
switch (step) {
2021-06-15 15:13:47 +02:00
case IMTQ::SELF_TEST_STEPS::INIT:
2022-01-18 11:41:19 +01:00
stepString = std::string("INIT");
break;
2021-06-15 15:13:47 +02:00
case IMTQ::SELF_TEST_STEPS::X_POSITIVE:
2022-01-18 11:41:19 +01:00
stepString = std::string("+X");
break;
2021-06-04 16:45:54 +02:00
case IMTQ::SELF_TEST_STEPS::X_NEGATIVE:
2022-01-18 11:41:19 +01:00
stepString = std::string("-X");
break;
2021-06-04 16:45:54 +02:00
case IMTQ::SELF_TEST_STEPS::Y_POSITIVE:
2022-01-18 11:41:19 +01:00
stepString = std::string("+Y");
break;
2021-06-04 16:45:54 +02:00
case IMTQ::SELF_TEST_STEPS::Y_NEGATIVE:
2022-01-18 11:41:19 +01:00
stepString = std::string("-Y");
break;
2021-06-04 16:45:54 +02:00
case IMTQ::SELF_TEST_STEPS::Z_POSITIVE:
2022-01-18 11:41:19 +01:00
stepString = std::string("+Z");
break;
2021-06-04 16:45:54 +02:00
case IMTQ::SELF_TEST_STEPS::Z_NEGATIVE:
2022-01-18 11:41:19 +01:00
stepString = std::string("-Z");
break;
2021-06-04 16:45:54 +02:00
case IMTQ::SELF_TEST_STEPS::FINA:
2022-01-18 11:41:19 +01:00
stepString = std::string("FINA");
break;
2021-06-04 16:45:54 +02:00
default:
2022-01-18 11:41:19 +01:00
sif::error << "IMTQHandler::checkErrorByte: Received packet with invalid step information"
<< std::endl;
break;
}
return stepString;
2021-06-04 16:45:54 +02:00
}