921 lines
43 KiB
C++
921 lines
43 KiB
C++
#include "IMTQHandler.h"
|
|
#include "OBSWConfig.h"
|
|
|
|
#include <fsfw/globalfunctions/CRC.h>
|
|
#include <fsfw/datapool/PoolReadGuard.h>
|
|
#include <math.h>
|
|
|
|
|
|
IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) :
|
|
DeviceHandlerBase(objectId, comIF, comCookie), engHkDataset(this), calMtmMeasurementSet(
|
|
this), rawMtmMeasurementSet(this), posXselfTestDataset(this) {
|
|
if (comCookie == NULL) {
|
|
sif::error << "IMTQHandler: Invalid com cookie" << std::endl;
|
|
}
|
|
}
|
|
|
|
IMTQHandler::~IMTQHandler() {
|
|
}
|
|
|
|
|
|
void IMTQHandler::doStartUp(){
|
|
#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
|
|
setMode(MODE_NORMAL);
|
|
#else
|
|
setMode(_MODE_TO_ON);
|
|
#endif
|
|
}
|
|
|
|
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){
|
|
return RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t IMTQHandler::buildCommandFromCommand(
|
|
DeviceCommandId_t deviceCommand, const uint8_t * commandData,
|
|
size_t commandDataLen) {
|
|
switch(deviceCommand) {
|
|
case(IMTQ::POS_X_SELF_TEST): {
|
|
commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD;
|
|
commandBuffer[1] = IMTQ::SELF_TEST_AXIS::X_POSITIVE;
|
|
rawPacket = commandBuffer;
|
|
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::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);
|
|
}
|
|
|
|
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;
|
|
result = getSelfTestCommandId(foundId);
|
|
break;
|
|
case(IMTQ::CC::GET_SELF_TEST_RESULT):
|
|
*foundLen = IMTQ::SIZE_SELF_TEST_RESULTS;
|
|
*foundId = IMTQ::GET_SELF_TEST_RESULT;
|
|
break;
|
|
default:
|
|
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::POS_X_SELF_TEST):
|
|
case (IMTQ::NEG_X_SELF_TEST):
|
|
case (IMTQ::POS_Y_SELF_TEST):
|
|
case (IMTQ::NEG_Y_SELF_TEST):
|
|
case (IMTQ::POS_Z_SELF_TEST):
|
|
case (IMTQ::NEG_Z_SELF_TEST):
|
|
case (IMTQ::START_ACTUATION_DIPOLE):
|
|
case (IMTQ::START_MTM_MEASUREMENT):
|
|
/* Replies only the status byte which is already handled with parseStatusByte */
|
|
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(){
|
|
|
|
}
|
|
|
|
LocalPoolDataSetBase* IMTQHandler::getDataSetHandle(sid_t sid) {
|
|
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 {
|
|
sif::error << "IMTQHandler::getDataSetHandle: Invalid sid" << std::endl;
|
|
return nullptr;
|
|
}
|
|
}
|
|
|
|
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<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 }));
|
|
|
|
/** Entries of dataset for self test results of 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 }));
|
|
|
|
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::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 }));
|
|
|
|
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 }));
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t IMTQHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
|
|
DeviceCommandId_t commandId = getPendingCommand();
|
|
switch(commandId) {
|
|
case IMTQ::POS_X_SELF_TEST:
|
|
case IMTQ::NEG_X_SELF_TEST:
|
|
case IMTQ::POS_Y_SELF_TEST:
|
|
case IMTQ::NEG_Y_SELF_TEST:
|
|
case IMTQ::POS_Z_SELF_TEST:
|
|
case IMTQ::NEG_Z_SELF_TEST:
|
|
*id = commandId;
|
|
break;
|
|
default:
|
|
sif::error << "IMTQHandler::getSelfTestCommandId: Reply does not match to pending "
|
|
<< "command" << std::endl;
|
|
return UNEXPECTED_SELF_TEST_REPLY;
|
|
}
|
|
return RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) {
|
|
uint8_t cmdErrorField = *(packet + 1) & 0xF;
|
|
switch (cmdErrorField) {
|
|
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) {
|
|
uint16_t offset = 2;
|
|
checkErrorByte(*(packet + offset), *(packet + offset + 1));
|
|
|
|
switch(*(packet + IMTQ::MAIN_STEP_OFFSET)) {
|
|
case IMTQ::SELF_TEST_STEPS::X_POSITIVE: {
|
|
handlePositiveXSelfTestReply(packet);
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void IMTQHandler::handlePositiveXSelfTestReply(const uint8_t* packet) {
|
|
PoolReadGuard rg(&posXselfTestDataset);
|
|
|
|
uint16_t offset = 2;
|
|
/** Init measurements */
|
|
posXselfTestDataset.initErr = *(packet + offset);
|
|
offset += 2; // STEP byte will not be stored
|
|
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 = (*(packet + offset + 1) << 8 | *(packet + offset))
|
|
* 0.1;
|
|
offset += 2;
|
|
posXselfTestDataset.initCoilYCurrent = (*(packet + offset + 1) << 8 | *(packet + offset))
|
|
* 0.1;
|
|
offset += 2;
|
|
posXselfTestDataset.initCoilZCurrent = (*(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 = (*(packet + offset + 1) << 8 | *(packet + offset))
|
|
* 0.1;
|
|
offset += 2;
|
|
posXselfTestDataset.coilYCurrent = (*(packet + offset + 1) << 8 | *(packet + offset))
|
|
* 0.1;
|
|
offset += 2;
|
|
posXselfTestDataset.coilZCurrent = (*(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 = (*(packet + offset + 1) << 8 | *(packet + offset))
|
|
* 0.1;
|
|
offset += 2;
|
|
posXselfTestDataset.finaCoilYCurrent = (*(packet + offset + 1) << 8 | *(packet + offset))
|
|
* 0.1;
|
|
offset += 2;
|
|
posXselfTestDataset.finaCoilZCurrent = (*(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 && IMTQ_DEBUG == 1
|
|
sif::info << "IMTQ self test (INIT) err: "
|
|
<< static_cast<unsigned int>(posXselfTestDataset.initErr.value) << std::endl;
|
|
sif::info << "IMTQ self test (INIT) raw magnetic field X: " << posXselfTestDataset.initRawMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << posXselfTestDataset.initRawMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << posXselfTestDataset.initRawMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " << posXselfTestDataset.initCalMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " << posXselfTestDataset.initCalMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " << posXselfTestDataset.initCalMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil X current: " << posXselfTestDataset.initCoilXCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Y current: " << posXselfTestDataset.initCoilYCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Z current: " << posXselfTestDataset.initCoilZCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil X temperature: "
|
|
<< posXselfTestDataset.initCoilXTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Y temperature: "
|
|
<< posXselfTestDataset.initCoilYTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (INIT) coil Z temperature: "
|
|
<< posXselfTestDataset.initCoilZTemperature << " °C" << std::endl;
|
|
|
|
sif::info << "IMTQ self test (+X) err: " <<
|
|
static_cast<unsigned int>(posXselfTestDataset.err.value) << std::endl;
|
|
sif::info << "IMTQ self test (+X) raw magnetic field X: " << posXselfTestDataset.rawMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+X) raw magnetic field Y: " << posXselfTestDataset.rawMagY
|
|
<< " 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
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+X) calibrated magnetic field Y: " << posXselfTestDataset.calMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+X) calibrated magnetic field Z: " << posXselfTestDataset.calMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (+X) coil X current: " << posXselfTestDataset.coilXCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (+X) coil Y current: " << posXselfTestDataset.coilYCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (+X) coil Z current: " << posXselfTestDataset.coilZCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (+X) coil X temperature: "
|
|
<< posXselfTestDataset.coilXTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (+X) coil Y temperature: "
|
|
<< posXselfTestDataset.coilYTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (+X) coil Z temperature: "
|
|
<< posXselfTestDataset.coilZTemperature << " °C" << std::endl;
|
|
|
|
sif::info << "IMTQ self test (FINA) err: "
|
|
<< static_cast<unsigned int>(posXselfTestDataset.finaErr.value) << std::endl;
|
|
sif::info << "IMTQ self test (FINA) raw magnetic field X: " << posXselfTestDataset.finaRawMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << posXselfTestDataset.finaRawMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << posXselfTestDataset.finaRawMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " << posXselfTestDataset.finaCalMagX
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " << posXselfTestDataset.finaCalMagY
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " << posXselfTestDataset.finaCalMagZ
|
|
<< " nT" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil X current: " << posXselfTestDataset.finaCoilXCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Y current: " << posXselfTestDataset.finaCoilYCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Z current: " << posXselfTestDataset.finaCoilZCurrent
|
|
<< " mA" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil X temperature: "
|
|
<< posXselfTestDataset.finaCoilXTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Y temperature: "
|
|
<< posXselfTestDataset.finaCoilYTemperature << " °C" << std::endl;
|
|
sif::info << "IMTQ self test (FINA) coil Z temperature: "
|
|
<< posXselfTestDataset.finaCoilZTemperature << " °C" << std::endl;
|
|
#endif
|
|
}
|
|
|
|
void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) {
|
|
std::string stepString("");
|
|
if (step < 8) {
|
|
stepString = makeStepString(step);
|
|
}
|
|
else {
|
|
/** This should normally never happen */
|
|
sif::debug << "IMTQHandler::checkErrorByte: Invalid step" << std::endl;
|
|
return;
|
|
}
|
|
|
|
if (errorByte == 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;
|
|
}
|
|
}
|
|
|
|
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;
|
|
}
|
|
|