eive/eive-obsw
eive/eive-obsw
12
4
Fork 0
Code Issues 4 Pull Requests Releases 120 Wiki Activity
0de484e423
eive-obsw/mission/devices/IMTQHandler.cpp

1163 lines
59 KiB
C++
Raw Blame History

#include "IMTQHandler.h"
#include "OBSWConfig.h"
#include <fsfw/globalfunctions/CRC.h>
#include <fsfw/datapool/PoolReadGuard.h>
IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) :
DeviceHandlerBase(objectId, comIF, comCookie), engHkDataset(this), calMtmMeasurementSet(
this), rawMtmMeasurementSet(this), selfTestDataset(this) {
if (comCookie == NULL) {
sif::error << "IMTQHandler: Invalid com cookie" << std::endl;
}
}
IMTQHandler::~IMTQHandler() {
}
void IMTQHandler::doStartUp(){
if(selfTestPerformed == true){
#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
setMode(MODE_NORMAL);
#else
setMode(_MODE_TO_ON);
#endif
selfTestPerformed = false;
}
}
void IMTQHandler::doShutDown(){
}
ReturnValue_t IMTQHandler::buildNormalDeviceCommand(
DeviceCommandId_t * id) {
switch (communicationStep) {
case CommunicationStep::GET_ENG_HK_DATA:
*id = IMTQ::GET_ENG_HK_DATA;
communicationStep = CommunicationStep::START_MTM_MEASUREMENT;
break;
case CommunicationStep::START_MTM_MEASUREMENT:
*id = IMTQ::START_MTM_MEASUREMENT;
communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT;
break;
case CommunicationStep::GET_CAL_MTM_MEASUREMENT:
*id = IMTQ::GET_CAL_MTM_MEASUREMENT;
communicationStep = CommunicationStep::GET_RAW_MTM_MEASUREMENT;
break;
case CommunicationStep::GET_RAW_MTM_MEASUREMENT:
*id = IMTQ::GET_RAW_MTM_MEASUREMENT;
communicationStep = CommunicationStep::GET_ENG_HK_DATA;
break;
default:
sif::debug << "IMTQHandler::buildNormalDeviceCommand: Invalid communication step"
<< std::endl;
break;
}
return buildCommandFromCommand(*id, NULL, 0);
}
ReturnValue_t IMTQHandler::buildTransitionDeviceCommand(
DeviceCommandId_t * id){
if (mode == _MODE_START_UP) {
switch (startupStep) {
case StartupStep::COMMAND_SELF_TEST:
*id = IMTQ::SELF_TEST_CMD;
startupStep = StartupStep::GET_SELF_TEST_RESULT;
break;
case StartupStep::GET_SELF_TEST_RESULT:
*id = IMTQ::GET_SELF_TEST_RESULT;
startupStep = StartupStep::NONE;
break;
default:
sif::debug << "IMTQHandler::buildTransitionDeviceCommand: Unknown startup step"
<< std::endl;
}
}
return buildCommandFromCommand(*id, nullptr, 0);;
}
ReturnValue_t IMTQHandler::buildCommandFromCommand(
DeviceCommandId_t deviceCommand, const uint8_t * commandData,
size_t commandDataLen) {
switch(deviceCommand) {
case(IMTQ::SELF_TEST_CMD): {
commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD;
commandBuffer[1] = IMTQ::SELF_TEST_AXIS::ALL;
rawPacket = commandBuffer;
rawPacketLen = 2;
return RETURN_OK;
}
case(IMTQ::GET_SELF_TEST_RESULT): {
commandBuffer[0] = IMTQ::CC::GET_SELF_TEST_RESULT;
rawPacket = commandBuffer;
rawPacketLen = 1;
return RETURN_OK;
}
case(IMTQ::START_ACTUATION_DIPOLE): {
/* IMTQ expects low byte first */
commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE;
commandBuffer[1] = *(commandData + 1);
commandBuffer[2] = *(commandData);
commandBuffer[3] = *(commandData + 3);
commandBuffer[4] = *(commandData + 2);
commandBuffer[5] = *(commandData + 5);
commandBuffer[6] = *(commandData + 4);
commandBuffer[7] = *(commandData + 7);
commandBuffer[8] = *(commandData + 6);
rawPacket = commandBuffer;
rawPacketLen = 9;
return RETURN_OK;
}
case(IMTQ::GET_ENG_HK_DATA): {
commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA;
rawPacket = commandBuffer;
rawPacketLen = 1;
return RETURN_OK;
}
case(IMTQ::GET_COMMANDED_DIPOLE): {
commandBuffer[0] = IMTQ::CC::GET_COMMANDED_DIPOLE;
rawPacket = commandBuffer;
rawPacketLen = 1;
return RETURN_OK;
}
case(IMTQ::START_MTM_MEASUREMENT): {
commandBuffer[0] = IMTQ::CC::START_MTM_MEASUREMENT;
rawPacket = commandBuffer;
rawPacketLen = 1;
return RETURN_OK;
}
case(IMTQ::GET_CAL_MTM_MEASUREMENT): {
commandBuffer[0] = IMTQ::CC::GET_CAL_MTM_MEASUREMENT;
rawPacket = commandBuffer;
rawPacketLen = 1;
return RETURN_OK;
}
case(IMTQ::GET_RAW_MTM_MEASUREMENT): {
commandBuffer[0] = IMTQ::CC::GET_RAW_MTM_MEASUREMENT;
rawPacket = commandBuffer;
rawPacketLen = 1;
return RETURN_OK;
}
default:
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
return HasReturnvaluesIF::RETURN_FAILED;
}
void IMTQHandler::fillCommandAndReplyMap() {
this->insertInCommandAndReplyMap(IMTQ::SELF_TEST_CMD, 1, nullptr,
IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::GET_SELF_TEST_RESULT, 1, nullptr,
IMTQ::SIZE_SELF_TEST_RESULTS);
this->insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr,
IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset,
IMTQ::SIZE_ENG_HK_DATA_REPLY);
this->insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr,
IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY);
this->insertInCommandAndReplyMap(IMTQ::START_MTM_MEASUREMENT, 1, nullptr,
IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::GET_CAL_MTM_MEASUREMENT, 1, &calMtmMeasurementSet,
IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT);
this->insertInCommandAndReplyMap(IMTQ::GET_RAW_MTM_MEASUREMENT, 1, &rawMtmMeasurementSet,
IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT);
}
ReturnValue_t IMTQHandler::scanForReply(const uint8_t *start,
size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) {
ReturnValue_t result = RETURN_OK;
switch(*start) {
case(IMTQ::CC::START_ACTUATION_DIPOLE):
*foundLen = IMTQ::SIZE_STATUS_REPLY;
*foundId = IMTQ::START_ACTUATION_DIPOLE;
break;
case(IMTQ::CC::START_MTM_MEASUREMENT):
*foundLen = IMTQ::SIZE_STATUS_REPLY;
*foundId = IMTQ::START_MTM_MEASUREMENT;
break;
case(IMTQ::CC::GET_ENG_HK_DATA):
*foundLen = IMTQ::SIZE_ENG_HK_DATA_REPLY;
*foundId = IMTQ::GET_ENG_HK_DATA;
break;
case(IMTQ::CC::GET_COMMANDED_DIPOLE):
*foundLen = IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY;
*foundId = IMTQ::GET_COMMANDED_DIPOLE;
break;
case(IMTQ::CC::GET_CAL_MTM_MEASUREMENT):
*foundLen = IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT;
*foundId = IMTQ::GET_CAL_MTM_MEASUREMENT;
break;
case(IMTQ::CC::GET_RAW_MTM_MEASUREMENT):
*foundLen = IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT;
*foundId = IMTQ::GET_RAW_MTM_MEASUREMENT;
break;
case(IMTQ::CC::SELF_TEST_CMD):
*foundLen = IMTQ::SIZE_STATUS_REPLY;
*foundId = IMTQ::SELF_TEST_CMD;
break;
case(IMTQ::CC::GET_SELF_TEST_RESULT):
*foundLen = IMTQ::SIZE_SELF_TEST_RESULTS;
*foundId = IMTQ::GET_SELF_TEST_RESULT;
break;
default:
sif::debug << "IMTQHandler::scanForReply: Reply contains invalid command code" << std::endl;
result = IGNORE_REPLY_DATA;
break;
}
return result;
}
ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) {
ReturnValue_t result = RETURN_OK;
result = parseStatusByte(packet);
if (result != RETURN_OK) {
return result;
}
switch (id) {
case (IMTQ::SELF_TEST_CMD):
case (IMTQ::START_ACTUATION_DIPOLE):
case (IMTQ::START_MTM_MEASUREMENT):
/* Replies only the status byte which is already handled with parseStatusByte */
break;
case (IMTQ::GET_ENG_HK_DATA):
fillEngHkDataset(packet);
break;
case (IMTQ::GET_COMMANDED_DIPOLE):
handleGetCommandedDipoleReply(packet);
break;
case (IMTQ::GET_CAL_MTM_MEASUREMENT):
fillCalibratedMtmDataset(packet);
break;
case (IMTQ::GET_RAW_MTM_MEASUREMENT):
fillRawMtmDataset(packet);
break;
case (IMTQ::GET_SELF_TEST_RESULT):
handleSelfTestReply(packet);
break;
default: {
sif::debug << "IMTQHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
}
}
return RETURN_OK;
}
void IMTQHandler::setNormalDatapoolEntriesInvalid(){
}
uint32_t IMTQHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo){
return 5000;
}
ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) {
/** Entries of engineering housekeeping dataset */
localDataPoolMap.emplace(IMTQ::DIGITAL_VOLTAGE_MV, new PoolEntry<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<uint32_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::MTM_CAL_Y, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::MTM_CAL_Z, new PoolEntry<uint32_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 }));
/** Entries of dataset for self test results */
localDataPoolMap.emplace(IMTQ::INIT_ERR, new PoolEntry<uint8_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::INIT_RAW_MAG_X, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::INIT_RAW_MAG_Y, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::INIT_RAW_MAG_Z, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::INIT_CAL_MAG_X, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::INIT_CAL_MAG_Y, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::INIT_CAL_MAG_Z, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::INIT_COIL_X_CURRENT, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::INIT_COIL_Y_CURRENT, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::INIT_COIL_Z_CURRENT, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::INIT_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::INIT_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::INIT_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<uint32_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::POS_X_RAW_MAG_Y, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::POS_X_RAW_MAG_Z, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::POS_X_CAL_MAG_X, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::POS_X_CAL_MAG_Y, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::POS_X_CAL_MAG_Z, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::POS_X_COIL_X_CURRENT, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::POS_X_COIL_Y_CURRENT, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::POS_X_COIL_Z_CURRENT, new PoolEntry<uint16_t>( { 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 }));
localDataPoolMap.emplace(IMTQ::FINA_ERR, new PoolEntry<uint8_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::FINA_RAW_MAG_X, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::FINA_RAW_MAG_Y, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::FINA_RAW_MAG_Z, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::FINA_CAL_MAG_X, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::FINA_CAL_MAG_Y, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::FINA_CAL_MAG_Z, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::FINA_COIL_X_CURRENT, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::FINA_COIL_Y_CURRENT, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::FINA_COIL_Z_CURRENT, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::FINA_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::FINA_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::FINA_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) {
uint8_t cmdErrorField = *(packet + 1) & 0xF;
switch (cmdErrorField) {
case 0:
return RETURN_OK;
case 1:
sif::error << "IMTQHandler::parseStatusByte: Command rejected without reason" << std::endl;
return REJECTED_WITHOUT_REASON;
case 2:
sif::error << "IMTQHandler::parseStatusByte: Command has invalid command code" << std::endl;
return INVALID_COMMAND_CODE;
case 3:
sif::error << "IMTQHandler::parseStatusByte: Command has missing parameter" << std::endl;
return PARAMETER_MISSING;
case 4:
sif::error << "IMTQHandler::parseStatusByte: Command has invalid parameter" << std::endl;
return PARAMETER_INVALID;
case 5:
sif::error << "IMTQHandler::parseStatusByte: CC unavailable" << std::endl;
return CC_UNAVAILABLE;
case 7:
sif::error << "IMTQHandler::parseStatusByte: IMQT replied internal processing error"
<< std::endl;
return INTERNAL_PROCESSING_ERROR;
default:
sif::error << "IMTQHandler::parseStatusByte: CMD Error field contains unknown error code "
<< cmdErrorField << std::endl;
return CMD_ERR_UNKNOWN;
}
}
void IMTQHandler::fillEngHkDataset(const uint8_t* packet) {
PoolReadGuard rg(&engHkDataset);
uint8_t offset = 2;
engHkDataset.digitalVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
engHkDataset.analogVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
engHkDataset.digitalCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
engHkDataset.analogCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
engHkDataset.coilXCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
engHkDataset.coilYCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
engHkDataset.coilZCurrentmA = (*(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 && IMTQ_DEBUG == 1
sif::info << "IMTQ digital voltage: " << engHkDataset.digitalVoltageMv << " mV" << std::endl;
sif::info << "IMTQ analog voltage: " << engHkDataset.analogVoltageMv << " mV" << std::endl;
sif::info << "IMTQ digital current: " << engHkDataset.digitalCurrentmA << " mA" << std::endl;
sif::info << "IMTQ analog current: " << engHkDataset.analogCurrentmA << " mA" << std::endl;
sif::info << "IMTQ coil X current: " << engHkDataset.coilXCurrentmA << " mA" << std::endl;
sif::info << "IMTQ coil Y current: " << engHkDataset.coilYCurrentmA << " mA" << std::endl;
sif::info << "IMTQ coil Z current: " << engHkDataset.coilZCurrentmA << " mA" << std::endl;
sif::info << "IMTQ coil X temperature: " << engHkDataset.coilXTemperature << " °C"
<< std::endl;
sif::info << "IMTQ coil Y temperature: " << engHkDataset.coilYTemperature << " °C"
<< std::endl;
sif::info << "IMTQ coil Z temperature: " << engHkDataset.coilZTemperature << " °C"
<< std::endl;
sif::info << "IMTQ coil MCU temperature: " << engHkDataset.mcuTemperature << " °C"
<< std::endl;
#endif
}
void IMTQHandler::setModeNormal() {
mode = MODE_NORMAL;
}
void IMTQHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) {
if (wiretappingMode == RAW) {
/* Data already sent in doGetRead() */
return;
}
DeviceReplyMap::iterator iter = deviceReplyMap.find(replyId);
if (iter == deviceReplyMap.end()) {
sif::debug << "IMTQHandler::handleDeviceTM: Unknown reply id" << std::endl;
return;
}
MessageQueueId_t queueId = iter->second.command->second.sendReplyTo;
if (queueId == NO_COMMANDER) {
return;
}
ReturnValue_t result = actionHelper.reportData(queueId, replyId, data, dataSize);
if (result != RETURN_OK) {
sif::debug << "IMTQHandler::handleDeviceTM: Failed to report data" << std::endl;
return;
}
}
void IMTQHandler::handleGetCommandedDipoleReply(const uint8_t* packet) {
uint8_t tmData[6];
/* Switching endianess of received dipole values */
tmData[0] = *(packet + 3);
tmData[1] = *(packet + 2);
tmData[2] = *(packet + 5);
tmData[3] = *(packet + 4);
tmData[4] = *(packet + 7);
tmData[5] = *(packet + 6);
handleDeviceTM(tmData, sizeof(tmData), IMTQ::GET_COMMANDED_DIPOLE);
}
void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
PoolReadGuard rg(&calMtmMeasurementSet);
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 && IMTQ_DEBUG == 1
sif::info << "IMTQ calibrated MTM measurement X: " << calMtmMeasurementSet.mtmXnT << " nT"
<< std::endl;
sif::info << "IMTQ calibrated MTM measurement Y: " << calMtmMeasurementSet.mtmYnT << " nT"
<< std::endl;
sif::info << "IMTQ calibrated MTM measurement Z: " << calMtmMeasurementSet.mtmZnT << " nT"
<< std::endl;
sif::info << "IMTQ coil actuation status during MTM measurement: "
<< (unsigned int) calMtmMeasurementSet.coilActuationStatus.value << std::endl;
#endif
}
void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) {
PoolReadGuard rg(&rawMtmMeasurementSet);
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 && IMTQ_DEBUG == 1
sif::info << "IMTQ raw MTM measurement X: " << rawMtmMeasurementSet.mtmXnT << " nT"
<< std::endl;
sif::info << "IMTQ raw MTM measurement Y: " << rawMtmMeasurementSet.mtmYnT << " nT"
<< 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;
#endif
}
void IMTQHandler::handleSelfTestReply(const uint8_t* packet) {
PoolReadGuard rg(&selfTestDataset);
/** Init measurements */
int8_t offset = 2;
checkErrorByte(*(packet + offset), *(packet + offset + 1));
selfTestDataset.initErr = *(packet + offset);
offset += 2; // STEP byte will not be stored
selfTestDataset.initRawMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.initRawMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.initRawMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.initCoilXCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.initCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.initCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
/** +X measurements */
checkErrorByte(*(packet + offset), *(packet + offset + 1));
selfTestDataset.posXErr = *(packet + offset);
offset += 2; // STEP byte will not be stored
selfTestDataset.posXRawMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.posXRawMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.posXRawMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.posXCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.posXCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.posXCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.posXCoilXCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.posXCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.posXCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.posXCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.posXCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.posXCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
/** -X measurements */
checkErrorByte(*(packet + offset), *(packet + offset + 1));
selfTestDataset.negXErr = *(packet + offset);
offset += 2; // STEP byte will not be stored
selfTestDataset.negXRawMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.negXRawMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.negXRawMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.negXCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.negXCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.negXCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.negXCoilXCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.negXCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.negXCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.negXCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.negXCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.negXCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
/** +Y measurements */
checkErrorByte(*(packet + offset), *(packet + offset + 1));
selfTestDataset.posYErr = *(packet + offset);
offset += 2; // STEP byte will not be stored
selfTestDataset.posYRawMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.posYRawMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.posYRawMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.posYCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.posYCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.posYCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.posYCoilXCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.posYCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.posYCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.posYCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.posYCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.posYCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
/** -Y measurements */
checkErrorByte(*(packet + offset), *(packet + offset + 1));
selfTestDataset.negYErr = *(packet + offset);
offset += 2; // STEP byte will not be stored
selfTestDataset.negYRawMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.negYRawMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.negYRawMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.negYCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.negYCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.negYCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.negYCoilXCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.negYCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.negYCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.negYCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.negYCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.negYCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
/** +Z measurements */
selfTestDataset.posZErr = *(packet + offset);
offset += 2; // STEP byte will not be stored
selfTestDataset.posZRawMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.posZRawMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.posZRawMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.posZCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.posZCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.posZCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.posZCoilXCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.posZCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.posZCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.posZCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.posZCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.posZCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
/** -Z measurements */
selfTestDataset.negZErr = *(packet + offset);
offset += 2; // STEP byte will not be stored
selfTestDataset.negZRawMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.negZRawMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.negZRawMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.negZCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.negZCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.negZCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.negZCoilXCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.negZCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.negZCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.negZCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.negZCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.negZCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
/** FINA measurements */
selfTestDataset.finaErr = *(packet + offset);
offset += 2; // STEP byte will not be stored
selfTestDataset.finaRawMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.finaRawMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.finaRawMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
selfTestDataset.finaCoilXCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.finaCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.finaCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 2;
selfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1
sif::info << "IMTQ self test (INIT) err: "
<< static_cast<unsigned int>(selfTestDataset.initErr.value) << std::endl;
sif::info << "IMTQ self test (INIT) raw magnetic field X: " << selfTestDataset.initRawMagX
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << selfTestDataset.initRawMagY
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << selfTestDataset.initRawMagZ
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " << selfTestDataset.initCalMagX
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " << selfTestDataset.initCalMagY
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " << selfTestDataset.initCalMagZ
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (INIT) coil X current: " << selfTestDataset.initCoilXCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (INIT) coil Y current: " << selfTestDataset.initCoilYCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (INIT) coil Z current: " << selfTestDataset.initCoilZCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (INIT) coil X temperature: "
<< selfTestDataset.initCoilXTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (INIT) coil Y temperature: "
<< selfTestDataset.initCoilYTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (INIT) coil Z temperature: "
<< selfTestDataset.initCoilZTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (+X) err: " <<
static_cast<unsigned int>(selfTestDataset.posXErr.value) << std::endl;
sif::info << "IMTQ self test (+X) raw magnetic field X: " << selfTestDataset.posXRawMagX
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (+X) raw magnetic field Y: " << selfTestDataset.posXRawMagY
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (+X) raw magnetic field Z: " << selfTestDataset.posXRawMagZ
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (+X) calibrated magnetic field X: " << selfTestDataset.posXCalMagX
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (+X) calibrated magnetic field Y: " << selfTestDataset.posXCalMagY
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (+X) calibrated magnetic field Z: " << selfTestDataset.posXCalMagZ
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (+X) coil X current: " << selfTestDataset.posXCoilXCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (+X) coil Y current: " << selfTestDataset.posXCoilYCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (+X) coil Z current: " << selfTestDataset.initCoilZCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (+X) coil X temperature: "
<< selfTestDataset.posXCoilXTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (+X) coil Y temperature: "
<< selfTestDataset.posXCoilYTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (+X) coil Z temperature: "
<< selfTestDataset.posXCoilZTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (-X) err: "
<< static_cast<unsigned int>(selfTestDataset.negXErr.value) << std::endl;
sif::info << "IMTQ self test (-X) raw magnetic field X: " << selfTestDataset.negXRawMagX
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (-X) raw magnetic field Y: " << selfTestDataset.negXRawMagY
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (-X) raw magnetic field Z: " << selfTestDataset.negXRawMagZ
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (-X) calibrated magnetic field X: " << selfTestDataset.negXCalMagX
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (-X) calibrated magnetic field Y: " << selfTestDataset.negXCalMagY
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (-X) calibrated magnetic field Z: " << selfTestDataset.negXCalMagZ
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (-X) coil X current: " << selfTestDataset.negXCoilXCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (-X) coil Y current: " << selfTestDataset.negXCoilYCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (-X) coil Z current: " << selfTestDataset.negXCoilZCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (-X) coil X temperature: "
<< selfTestDataset.negXCoilXTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (-X) coil Y temperature: "
<< selfTestDataset.negXCoilYTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (-X) coil Z temperature: "
<< selfTestDataset.negXCoilZTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (+Y) err: "
<< static_cast<unsigned int>(selfTestDataset.posYErr.value) << std::endl;
sif::info << "IMTQ self test (+Y) raw magnetic field X: " << selfTestDataset.posYRawMagX
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (+Y) raw magnetic field Y: " << selfTestDataset.posYRawMagY
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (+Y) raw magnetic field Z: " << selfTestDataset.posYRawMagZ
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (+Y) calibrated magnetic field X: " << selfTestDataset.posYCalMagX
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (+Y) calibrated magnetic field Y: " << selfTestDataset.posYCalMagY
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (+Y) calibrated magnetic field Z: " << selfTestDataset.posYCalMagZ
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (+Y) coil X current: " << selfTestDataset.posYCoilXCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (+Y) coil Y current: " << selfTestDataset.posYCoilYCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (+Y) coil Z current: " << selfTestDataset.posYCoilZCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (+Y) coil X temperature: "
<< selfTestDataset.posYCoilXTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (+Y) coil Y temperature: "
<< selfTestDataset.posYCoilYTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (+Y) coil Z temperature: "
<< selfTestDataset.posYCoilZTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (-Y) err: "
<< static_cast<unsigned int>(selfTestDataset.negYErr.value) << std::endl;
sif::info << "IMTQ self test (-Y) raw magnetic field X: " << selfTestDataset.negYRawMagX
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (-Y) raw magnetic field Y: " << selfTestDataset.negYRawMagY
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (-Y) raw magnetic field Z: " << selfTestDataset.negYRawMagZ
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (-Y) calibrated magnetic field X: " << selfTestDataset.negYCalMagX
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (-Y) calibrated magnetic field Y: " << selfTestDataset.negYCalMagY
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (-Y) calibrated magnetic field Z: " << selfTestDataset.negYCalMagZ
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (-Y) coil X current: " << selfTestDataset.negYCoilXCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (-Y) coil Y current: " << selfTestDataset.negYCoilYCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (-Y) coil Z current: " << selfTestDataset.negYCoilZCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (-Y) coil X temperature: "
<< selfTestDataset.negYCoilXTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (-Y) coil Y temperature: "
<< selfTestDataset.negYCoilYTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (-Y) coil Z temperature: "
<< selfTestDataset.negYCoilZTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (+Z) err: "
<< static_cast<unsigned int>(selfTestDataset.posZErr.value) << std::endl;
sif::info << "IMTQ self test (+Z) raw magnetic field X: " << selfTestDataset.posZRawMagX
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (+Z) raw magnetic field Y: " << selfTestDataset.posZRawMagY
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (+Z) raw magnetic field Z: " << selfTestDataset.posZRawMagZ
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (+Z) calibrated magnetic field X: " << selfTestDataset.posZCalMagX
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (+Z) calibrated magnetic field Y: " << selfTestDataset.posZCalMagY
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (+Z) calibrated magnetic field Z: " << selfTestDataset.posZCalMagZ
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (+Z) coil X current: " << selfTestDataset.posZCoilXCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (+Z) coil Y current: " << selfTestDataset.posZCoilYCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (+Z) coil Z current: " << selfTestDataset.posZCoilZCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (+Z) coil X temperature: "
<< selfTestDataset.posZCoilXTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (+Z) coil Y temperature: "
<< selfTestDataset.posZCoilYTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (+Z) coil Z temperature: "
<< selfTestDataset.posZCoilZTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (-Z) err: "
<< static_cast<unsigned int>(selfTestDataset.negZErr.value) << std::endl;
sif::info << "IMTQ self test (-Z) raw magnetic field X: " << selfTestDataset.negZRawMagX
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (-Z) raw magnetic field Y: " << selfTestDataset.negZRawMagY
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (-Z) raw magnetic field Z: " << selfTestDataset.negZRawMagZ
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (-Z) calibrated magnetic field X: " << selfTestDataset.negZCalMagX
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (-Z) calibrated magnetic field Y: " << selfTestDataset.negZCalMagY
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (-Z) calibrated magnetic field Z: " << selfTestDataset.negZCalMagZ
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (-Z) coil X current: " << selfTestDataset.negZCoilXCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (-Z) coil Y current: " << selfTestDataset.negZCoilYCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (-Z) coil Z current: " << selfTestDataset.negZCoilZCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (-Z) coil X temperature: "
<< selfTestDataset.negZCoilXTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (-Z) coil Y temperature: "
<< selfTestDataset.negZCoilYTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (-Z) coil Z temperature: "
<< selfTestDataset.negZCoilZTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (FINA) err: "
<< static_cast<unsigned int>(selfTestDataset.finaErr.value) << std::endl;
sif::info << "IMTQ self test (FINA) raw magnetic field X: " << selfTestDataset.finaRawMagX
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << selfTestDataset.finaRawMagY
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << selfTestDataset.finaRawMagZ
<< " 7.5 * 10^-9 T" << std::endl;
sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " << selfTestDataset.finaCalMagX
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " << selfTestDataset.finaCalMagY
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " << selfTestDataset.finaCalMagZ
<< " 10^-9 T" << std::endl;
sif::info << "IMTQ self test (FINA) coil X current: " << selfTestDataset.finaCoilXCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (FINA) coil Y current: " << selfTestDataset.finaCoilYCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (FINA) coil Z current: " << selfTestDataset.finaCoilZCurrent
<< " 10^-4 A" << std::endl;
sif::info << "IMTQ self test (FINA) coil X temperature: "
<< selfTestDataset.finaCoilXTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (FINA) coil Y temperature: "
<< selfTestDataset.finaCoilYTemperature << " °C" << std::endl;
sif::info << "IMTQ self test (FINA) coil Z temperature: "
<< selfTestDataset.finaCoilZTemperature << " °C" << std::endl;
#endif
if (mode == _MODE_START_UP) {
selfTestPerformed = true;
}
}
void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) {
std::string stepString("");
if (step < 8) {
stepString = makeStepString(step);
}
else {
/** This should normally never happen */
sif::debug << "IMTQHandler::checkErrorByte: Invalid step" << std::endl;
return;
}
if (errorByte & IMTQ::I2C_FAILURE_MASK) {
triggerEvent(SELF_TEST_I2C_FAILURE, step);
sif::error << "IMTQHandler::checkErrorByte: Self test I2C failure for step " << stepString
<< std::endl;
}
if (errorByte & IMTQ::SPI_FAILURE_MASK) {
triggerEvent(SELF_TEST_SPI_FAILURE, step);
sif::error << "IMTQHandler::checkErrorByte: Self test SPI failure for step " << stepString
<< std::endl;
}
if (errorByte & IMTQ::ADC_FAILURE_MASK) {
triggerEvent(SELF_TEST_ADC_FAILURE, step);
sif::error << "IMTQHandler::checkErrorByte: Self test ADC failure for step " << stepString
<< std::endl;
}
if (errorByte & IMTQ::PWM_FAILURE_MASK) {
triggerEvent(SELF_TEST_PWM_FAILURE, step);
sif::error << "IMTQHandler::checkErrorByte: Self test PWM failure for step " << stepString
<< std::endl;
}
if (errorByte & IMTQ::TC_FAILURE_MASK) {
triggerEvent(SELF_TEST_TC_FAILURE, step);
sif::error << "IMTQHandler::checkErrorByte: Self test TC failure (system failure) for step "
<< stepString << std::endl;
}
if (errorByte & IMTQ::MTM_RANGE_FAILURE_MASK) {
triggerEvent(SELF_TEST_TC_FAILURE, step);
sif::error << "IMTQHandler::checkErrorByte: Self test MTM range failure for step "
<< stepString << std::endl;
}
if (errorByte & IMTQ::COIL_CURRENT_FAILURE_MASK) {
triggerEvent(SELF_TEST_COIL_CURRENT_FAILURE, step);
sif::error << "IMTQHandler::checkErrorByte: Self test coil current outside of expected "
"range for step " << stepString << std::endl;
}
}
std::string IMTQHandler::makeStepString(const uint8_t step) {
std::string stepString("");
switch (step) {
case IMTQ::SELF_TEST_STEPS::INIT:
stepString = std::string("INIT");
break;
case IMTQ::SELF_TEST_STEPS::X_POSITIVE:
stepString = std::string("+X");
break;
case IMTQ::SELF_TEST_STEPS::X_NEGATIVE:
stepString = std::string("-X");
break;
case IMTQ::SELF_TEST_STEPS::Y_POSITIVE:
stepString = std::string("+Y");
break;
case IMTQ::SELF_TEST_STEPS::Y_NEGATIVE:
stepString = std::string("-Y");
break;
case IMTQ::SELF_TEST_STEPS::Z_POSITIVE:
stepString = std::string("+Z");
break;
case IMTQ::SELF_TEST_STEPS::Z_NEGATIVE:
stepString = std::string("-Z");
break;
case IMTQ::SELF_TEST_STEPS::FINA:
stepString = std::string("FINA");
break;
default:
sif::error << "IMTQHandler::checkErrorByte: Received packet with invalid step information"
<< std::endl;
break;
}
return stepString;
}
Reference in New Issue View Git Blame Copy Permalink