#include "IMTQHandler.h" #include "OBSWConfig.h" #include #include IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) : DeviceHandlerBase(objectId, comIF, comCookie), engHkDataset(this), calMtmMeasurementSet( this), rawMtmMeasurementSet(this), posXselfTestDataset(this) { 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(){ } 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( { 0 })); localDataPoolMap.emplace(IMTQ::ANALOG_VOLTAGE_MV, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::DIGITAL_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::ANALOG_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::COIL_X_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::COIL_Y_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::COIL_Z_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::COIL_X_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry( { 0 })); /** Entries of calibrated MTM measurement dataset */ localDataPoolMap.emplace(IMTQ::MTM_CAL_X, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::MTM_CAL_Y, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::MTM_CAL_Z, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry( { 0 })); /** Entries of raw MTM measurement dataset */ localDataPoolMap.emplace(IMTQ::MTM_RAW_X, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::MTM_RAW_Y, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::MTM_RAW_Z, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry( { 0 })); /** Entries of dataset for self test results of positive X axis test */ localDataPoolMap.emplace(IMTQ::INIT_POS_X_ERR, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::INIT_POS_X_RAW_MAG_X, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::INIT_POS_X_RAW_MAG_Y, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::INIT_POS_X_RAW_MAG_Z, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::INIT_POS_X_CAL_MAG_X, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::INIT_POS_X_CAL_MAG_Y, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::INIT_POS_X_CAL_MAG_Z, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_X_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_Y_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_Z_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_X_ERR, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_X_RAW_MAG_X, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_X_RAW_MAG_Y, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_X_RAW_MAG_Z, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_X_CAL_MAG_X, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_X_CAL_MAG_Y, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_X_CAL_MAG_Z, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_X_COIL_X_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_X_COIL_Y_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_X_COIL_Z_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_X_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_X_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_X_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::INIT_POS_X_ERR, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::FINA_POS_X_RAW_MAG_X, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::FINA_POS_X_RAW_MAG_Y, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::FINA_POS_X_RAW_MAG_Z, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::FINA_POS_X_CAL_MAG_X, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::FINA_POS_X_CAL_MAG_Y, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::FINA_POS_X_CAL_MAG_Z, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_X_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Y_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Z_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_X_ERR, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_X_RAW_MAG_X, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_X_RAW_MAG_Y, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_X_RAW_MAG_Z, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_X_CAL_MAG_X, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_X_CAL_MAG_Y, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_X_CAL_MAG_Z, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_X_COIL_X_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_X_COIL_Y_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_X_COIL_Z_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_X_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_X_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_X_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Y_ERR, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Y_RAW_MAG_X, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Y_RAW_MAG_Y, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Y_RAW_MAG_Z, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Y_CAL_MAG_X, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Y_CAL_MAG_Y, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Y_CAL_MAG_Z, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Y_COIL_X_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Y_COIL_Y_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Y_COIL_Z_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Y_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Y_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Y_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Y_ERR, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Y_RAW_MAG_X, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Y_RAW_MAG_Y, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Y_RAW_MAG_Z, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Y_CAL_MAG_X, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Y_CAL_MAG_Y, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Y_CAL_MAG_Z, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_X_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_Y_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_Z_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Z_ERR, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Z_RAW_MAG_X, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Z_RAW_MAG_Y, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Z_RAW_MAG_Z, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Z_CAL_MAG_X, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Z_CAL_MAG_Y, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Z_CAL_MAG_Z, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Z_COIL_X_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Z_COIL_Y_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Z_COIL_Z_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Z_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Z_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::POS_Z_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Z_ERR, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Z_RAW_MAG_X, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Z_RAW_MAG_Y, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Z_RAW_MAG_Z, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Z_CAL_MAG_X, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Z_CAL_MAG_Y, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Z_CAL_MAG_Z, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_X_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_Y_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_Z_CURRENT, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t IMTQHandler::getSelfTestCommandId(DeviceCommandId_t* id) { DeviceCommandId_t commandId = getPendingCommand(); switch(commandId) { case IMTQ::POS_X_SELF_TEST: case IMTQ::NEG_X_SELF_TEST: case IMTQ::POS_Y_SELF_TEST: case IMTQ::NEG_Y_SELF_TEST: case IMTQ::POS_Z_SELF_TEST: case IMTQ::NEG_Z_SELF_TEST: *id = commandId; break; default: sif::error << "IMTQHandler::getSelfTestCommandId: Reply does not match to pending " << "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); offset += 4; posXselfTestDataset.initRawMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posXselfTestDataset.initRawMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); 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); offset += 2; posXselfTestDataset.initCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; posXselfTestDataset.initCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset); 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); offset += 4; posXselfTestDataset.rawMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posXselfTestDataset.rawMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); 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); offset += 2; posXselfTestDataset.coilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; posXselfTestDataset.coilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset); 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); offset += 4; posXselfTestDataset.finaRawMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; posXselfTestDataset.finaRawMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); 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); offset += 2; posXselfTestDataset.finaCoilYCurrent = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; posXselfTestDataset.finaCoilZCurrent = *(packet + offset + 1) << 8 | *(packet + offset); 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(posXselfTestDataset.initErr.value) << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field X: " << posXselfTestDataset.initRawMagX << " 7.5 * 10^-9 T" << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << posXselfTestDataset.initRawMagY << " 7.5 * 10^-9 T" << std::endl; sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << posXselfTestDataset.initRawMagZ << " 7.5 * 10^-9 T" << std::endl; sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " << posXselfTestDataset.initCalMagX << " 10^-9 T" << std::endl; sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " << posXselfTestDataset.initCalMagY << " 10^-9 T" << std::endl; sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " << posXselfTestDataset.initCalMagZ << " 10^-9 T" << std::endl; sif::info << "IMTQ self test (INIT) coil X current: " << posXselfTestDataset.initCoilXCurrent << " * 10^-4 A" << std::endl; sif::info << "IMTQ self test (INIT) coil Y current: " << posXselfTestDataset.initCoilYCurrent << " * 10^-4 A" << std::endl; sif::info << "IMTQ self test (INIT) coil Z current: " << posXselfTestDataset.initCoilZCurrent << " * 10^-4 A" << std::endl; sif::info << "IMTQ self test (INIT) coil X temperature: " << 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(posXselfTestDataset.err.value) << std::endl; sif::info << "IMTQ self test (+X) raw magnetic field X: " << posXselfTestDataset.rawMagX << " 7.5 * 10^-9 T" << std::endl; sif::info << "IMTQ self test (+X) raw magnetic field Y: " << posXselfTestDataset.rawMagY << " 7.5 * 10^-9 T" << std::endl; sif::info << "IMTQ self test (+X) raw magnetic field Z: " << posXselfTestDataset.rawMagZ << " 7.5 * 10^-9 T" << std::endl; sif::info << "IMTQ self test (+X) calibrated magnetic field X: " << posXselfTestDataset.calMagX << " 10^-9 T" << std::endl; sif::info << "IMTQ self test (+X) calibrated magnetic field Y: " << posXselfTestDataset.calMagY << " 10^-9 T" << std::endl; sif::info << "IMTQ self test (+X) calibrated magnetic field Z: " << posXselfTestDataset.calMagZ << " 10^-9 T" << std::endl; sif::info << "IMTQ self test (+X) coil X current: " << posXselfTestDataset.coilXCurrent << " * 10^-4 A" << std::endl; sif::info << "IMTQ self test (+X) coil Y current: " << posXselfTestDataset.coilYCurrent << " * 10^-4 A" << std::endl; sif::info << "IMTQ self test (+X) coil Z current: " << posXselfTestDataset.coilZCurrent << " * 10^-4 A" << std::endl; sif::info << "IMTQ self test (+X) coil X temperature: " << posXselfTestDataset.coilXTemperature << " °C" << std::endl; sif::info << "IMTQ self test (+X) coil Y temperature: " << posXselfTestDataset.coilYTemperature << " °C" << std::endl; sif::info << "IMTQ self test (+X) coil Z temperature: " << posXselfTestDataset.coilZTemperature << " °C" << std::endl; sif::info << "IMTQ self test (FINA) err: " << static_cast(posXselfTestDataset.finaErr.value) << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field X: " << posXselfTestDataset.finaRawMagX << " 7.5 * 10^-9 T" << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << posXselfTestDataset.finaRawMagY << " 7.5 * 10^-9 T" << std::endl; sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << posXselfTestDataset.finaRawMagZ << " 7.5 * 10^-9 T" << std::endl; sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " << posXselfTestDataset.finaCalMagX << " 10^-9 T" << std::endl; sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " << posXselfTestDataset.finaCalMagY << " 10^-9 T" << std::endl; sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " << posXselfTestDataset.finaCalMagZ << " 10^-9 T" << std::endl; sif::info << "IMTQ self test (FINA) coil X current: " << posXselfTestDataset.finaCoilXCurrent << " * 10^-4 A" << std::endl; sif::info << "IMTQ self test (FINA) coil Y current: " << posXselfTestDataset.finaCoilYCurrent << " * 10^-4 A" << std::endl; sif::info << "IMTQ self test (FINA) coil Z current: " << posXselfTestDataset.finaCoilZCurrent << " * 10^-4 A" << std::endl; sif::info << "IMTQ self test (FINA) coil X temperature: " << 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; }