diff --git a/bsp_q7s/InitMission.cpp b/bsp_q7s/InitMission.cpp index cc3ea26e..9a475a11 100644 --- a/bsp_q7s/InitMission.cpp +++ b/bsp_q7s/InitMission.cpp @@ -31,6 +31,7 @@ ServiceInterfaceStream sif::error("ERROR", true, false, true); ObjectManagerIF *objectManager = nullptr; void initmission::initMission() { + sif::info << "Building global objects.." << std::endl; /* Instantiate global object manager and also create all objects */ ObjectManager::instance()->setObjectFactoryFunction(ObjectFactory::produce, nullptr); @@ -84,6 +85,23 @@ void initmission::initTasks() { initmission::printAddObjectError("UDP_POLLING", objects::UDP_POLLING_TASK); } + /* UDP bridge */ + PeriodicTaskIF* errorReporterTestTask = factory->createPeriodicTask( + "ERROR_REPORTER", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); + result = udpBridgeTask->addComponent(objects::INTERNAL_ERROR_REPORTER); + if(result != HasReturnvaluesIF::RETURN_OK) { + initmission::printAddObjectError("ERROR_REPORTER", objects::INTERNAL_ERROR_REPORTER); + } + +#if TEST_CCSDS_BRIDGE == 1 + PeriodicTaskIF* ptmeTestTask = factory->createPeriodicTask( + "PTME_TEST", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc); + result = ptmeTestTask->addComponent(objects::CCSDS_IP_CORE_BRIDGE); + if(result != HasReturnvaluesIF::RETURN_OK) { + initmission::printAddObjectError("PTME_TEST", objects::CCSDS_IP_CORE_BRIDGE); + } +#endif + /* PUS Services */ PeriodicTaskIF* pusVerification = factory->createPeriodicTask( "PUS_VERIF", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); @@ -208,8 +226,14 @@ void initmission::initTasks() { pusMedPrio->startTask(); pusLowPrio->startTask(); + errorReporterTestTask->startTask(); + #if OBSW_ADD_TEST_CODE == 1 testTask->startTask(); #endif + +#if TEST_CCSDS_BRIDGE == 1 + ptmeTestTask->startTask(); +#endif sif::info << "Tasks started.." << std::endl; } diff --git a/bsp_q7s/ObjectFactory.cpp b/bsp_q7s/ObjectFactory.cpp index 178a6f97..99ec65c2 100644 --- a/bsp_q7s/ObjectFactory.cpp +++ b/bsp_q7s/ObjectFactory.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include "fsfw_hal/linux/uart/UartComIF.h" #include "fsfw_hal/linux/uart/UartCookie.h" @@ -401,61 +402,60 @@ void ObjectFactory::produce(void* args){ #if Q7S_ADD_RTD_DEVICES == 1 GpioCookie* rtdGpioCookie = new GpioCookie; - gpioCallbacks::initTcsBoardDecoder(gpioComIF); GpioCallback* gpioRtdIc3 = new GpioCallback(std::string("Chip select RTD IC3"), gpio::OUT, 1, - &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + &gpioCallbacks::spiCsDecoderCallback, gpioComIF); rtdGpioCookie->addGpio(gpioIds::RTD_IC3, gpioRtdIc3); GpioCallback* gpioRtdIc4 = new GpioCallback(std::string("Chip select RTD IC4"), gpio::OUT, 1, - &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + &gpioCallbacks::spiCsDecoderCallback, gpioComIF); rtdGpioCookie->addGpio(gpioIds::RTD_IC4, gpioRtdIc4); GpioCallback* gpioRtdIc5 = new GpioCallback(std::string("Chip select RTD IC5"), gpio::OUT, 1, - &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + &gpioCallbacks::spiCsDecoderCallback, gpioComIF); rtdGpioCookie->addGpio(gpioIds::RTD_IC5, gpioRtdIc5); GpioCallback* gpioRtdIc6 = new GpioCallback(std::string("Chip select RTD IC6"), gpio::OUT, 1, - &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + &gpioCallbacks::spiCsDecoderCallback, gpioComIF); rtdGpioCookie->addGpio(gpioIds::RTD_IC6, gpioRtdIc6); GpioCallback* gpioRtdIc7 = new GpioCallback(std::string("Chip select RTD IC7"), gpio::OUT, 1, - &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + &gpioCallbacks::spiCsDecoderCallback, gpioComIF); rtdGpioCookie->addGpio(gpioIds::RTD_IC7, gpioRtdIc7); GpioCallback* gpioRtdIc8 = new GpioCallback(std::string("Chip select RTD IC8"), gpio::OUT, 1, - &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + &gpioCallbacks::spiCsDecoderCallback, gpioComIF); rtdGpioCookie->addGpio(gpioIds::RTD_IC8, gpioRtdIc8); GpioCallback* gpioRtdIc9 = new GpioCallback(std::string("Chip select RTD IC9"), gpio::OUT, 1, - &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + &gpioCallbacks::spiCsDecoderCallback, gpioComIF); rtdGpioCookie->addGpio(gpioIds::RTD_IC9, gpioRtdIc9); GpioCallback* gpioRtdIc10 = new GpioCallback(std::string("Chip select RTD IC10"), gpio::OUT, 1, - &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + &gpioCallbacks::spiCsDecoderCallback, gpioComIF); rtdGpioCookie->addGpio(gpioIds::RTD_IC10, gpioRtdIc10); GpioCallback* gpioRtdIc11 = new GpioCallback(std::string("Chip select RTD IC11"), gpio::OUT, 1, - &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + &gpioCallbacks::spiCsDecoderCallback, gpioComIF); rtdGpioCookie->addGpio(gpioIds::RTD_IC11, gpioRtdIc11); GpioCallback* gpioRtdIc12 = new GpioCallback(std::string("Chip select RTD IC12"), gpio::OUT, 1, - &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + &gpioCallbacks::spiCsDecoderCallback, gpioComIF); rtdGpioCookie->addGpio(gpioIds::RTD_IC12, gpioRtdIc12); GpioCallback* gpioRtdIc13 = new GpioCallback(std::string("Chip select RTD IC13"), gpio::OUT, 1, - &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + &gpioCallbacks::spiCsDecoderCallback, gpioComIF); rtdGpioCookie->addGpio(gpioIds::RTD_IC13, gpioRtdIc13); GpioCallback* gpioRtdIc14 = new GpioCallback(std::string("Chip select RTD IC14"), gpio::OUT, 1, - &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + &gpioCallbacks::spiCsDecoderCallback, gpioComIF); rtdGpioCookie->addGpio(gpioIds::RTD_IC14, gpioRtdIc14); GpioCallback* gpioRtdIc15 = new GpioCallback(std::string("Chip select RTD IC15"), gpio::OUT, 1, - &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + &gpioCallbacks::spiCsDecoderCallback, gpioComIF); rtdGpioCookie->addGpio(gpioIds::RTD_IC15, gpioRtdIc15); GpioCallback* gpioRtdIc16 = new GpioCallback(std::string("Chip select RTD IC16"), gpio::OUT, 1, - &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + &gpioCallbacks::spiCsDecoderCallback, gpioComIF); rtdGpioCookie->addGpio(gpioIds::RTD_IC16, gpioRtdIc16); GpioCallback* gpioRtdIc17 = new GpioCallback(std::string("Chip select RTD IC17"), gpio::OUT, 1, - &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + &gpioCallbacks::spiCsDecoderCallback, gpioComIF); rtdGpioCookie->addGpio(gpioIds::RTD_IC17, gpioRtdIc17); GpioCallback* gpioRtdIc18 = new GpioCallback(std::string("Chip select RTD IC18"), gpio::OUT, 1, - &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + &gpioCallbacks::spiCsDecoderCallback, gpioComIF); rtdGpioCookie->addGpio(gpioIds::RTD_IC18, gpioRtdIc18); gpioComIF->addGpios(rtdGpioCookie); SpiCookie* spiRtdIc3 = new SpiCookie(addresses::RTD_IC3, gpioIds::RTD_IC3, std::string("/dev/spidev2.0"), Max31865Definitions::MAX_REPLY_SIZE, - spi::SpiModes::MODE_1, 2000000); + spi::SpiModes::MODE_3, 2000000); SpiCookie* spiRtdIc4 = new SpiCookie(addresses::RTD_IC4, gpioIds::RTD_IC4, std::string("/dev/spidev2.0"), Max31865Definitions::MAX_REPLY_SIZE, spi::SpiModes::MODE_1, 2000000); @@ -491,7 +491,7 @@ void ObjectFactory::produce(void* args){ spi::SpiModes::MODE_1, 2000000); SpiCookie* spiRtdIc15 = new SpiCookie(addresses::RTD_IC15, gpioIds::RTD_IC15, std::string("/dev/spidev2.0"), Max31865Definitions::MAX_REPLY_SIZE, - spi::SpiModes::MODE_1, 2000000); + spi::SpiModes::MODE_1, 3900000); SpiCookie* spiRtdIc16 = new SpiCookie(addresses::RTD_IC16, gpioIds::RTD_IC16, std::string("/dev/spidev2.0"), Max31865Definitions::MAX_REPLY_SIZE, spi::SpiModes::MODE_1, 2000000); @@ -518,7 +518,7 @@ void ObjectFactory::produce(void* args){ Max31865PT1000Handler* rtdIc16 = new Max31865PT1000Handler(objects::RTD_IC16, objects::SPI_COM_IF, spiRtdIc16, 0); Max31865PT1000Handler* rtdIc17 = new Max31865PT1000Handler(objects::RTD_IC17, objects::SPI_COM_IF, spiRtdIc17, 0); Max31865PT1000Handler* rtdIc18 = new Max31865PT1000Handler(objects::RTD_IC18, objects::SPI_COM_IF, spiRtdIc18, 0); -// rtdIc10->setStartUpImmediately(); + rtdIc17->setStartUpImmediately(); // rtdIc4->setStartUpImmediately(); (void) rtdIc3; @@ -535,16 +535,14 @@ void ObjectFactory::produce(void* args){ (void) rtdIc14; (void) rtdIc15; (void) rtdIc16; - (void) rtdIc17; +// (void) rtdIc17; (void) rtdIc18; #endif /* Q7S_ADD_RTD_DEVICES == 1 */ I2cCookie* imtqI2cCookie = new I2cCookie(addresses::IMTQ, IMTQ::MAX_REPLY_SIZE, std::string("/dev/i2c-0")); - IMTQHandler* imtqHandler = new IMTQHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie); -// imtqHandler->setStartUpImmediately(); - (void) imtqHandler; + new IMTQHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie); UartCookie* plocUartCookie = new UartCookie(objects::PLOC_HANDLER, std::string("/dev/ttyUL3"), UartModes::NON_CANONICAL, 115200, PLOC::MAX_REPLY_SIZE); @@ -577,9 +575,22 @@ void ObjectFactory::produce(void* args){ SpiCookie* spiCookieSus = new SpiCookie(addresses::SUS_1, std::string("/dev/spidev1.0"), SUS::MAX_CMD_SIZE, spi::DEFAULT_MAX_1227_MODE, spi::DEFAULT_MAX_1227_SPEED); - SusHandler* sus1 = new SusHandler(objects::SUS_1, objects::SPI_COM_IF, spiCookieSus, gpioComIF, + new SusHandler(objects::SUS_1, objects::SPI_COM_IF, spiCookieSus, gpioComIF, gpioIds::CS_SUS_1); - sus1->setStartUpImmediately(); +#endif + +#if TE0720 == 1 && TEST_CCSDS_BRIDGE == 1 + GpioCookie* gpioCookieCcsdsIp = new GpioCookie; + GpiodRegular* papbBusyN = new GpiodRegular(std::string("gpiochip0"), 0, std::string("PAPBBusy_N")); + gpioCookieCcsdsIp->addGpio(gpioIds::PAPB_BUSY_N, papbBusyN); + GpiodRegular* papbEmpty = new GpiodRegular(std::string("gpiochip0"), 1, + std::string("Chip Select Sus Sensor")); + gpioCookieCcsdsIp->addGpio(gpioIds::PAPB_EMPTY, papbEmpty); + gpioComIF->addGpios(gpioCookieCcsdsIp); + + new CCSDSIPCoreBridge(objects::CCSDS_IP_CORE_BRIDGE, objects::CCSDS_PACKET_DISTRIBUTOR, + objects::TM_STORE, objects::TC_STORE, gpioComIF, std::string("/dev/uio0"), + gpioIds::PAPB_BUSY_N, gpioIds::PAPB_EMPTY); #endif #if TE0720 == 1 && TEST_RADIATION_SENSOR_HANDLER == 1 diff --git a/common/config/commonClassIds.h b/common/config/commonClassIds.h index 8ec68622..7a53bdf0 100644 --- a/common/config/commonClassIds.h +++ b/common/config/commonClassIds.h @@ -15,6 +15,7 @@ enum commonClassIds: uint8_t { IMTQ_HANDLER, //IMTQ PLOC_HANDLER, //PLOC SUS_HANDLER, //SUSS + CCSDS_IP_CORE_BRIDGE, // IP Core interface COMMON_CLASS_ID_END // [EXPORT] : [END] }; diff --git a/common/config/commonSubsystemIds.h b/common/config/commonSubsystemIds.h index e06de2a1..0bc3e1dd 100644 --- a/common/config/commonSubsystemIds.h +++ b/common/config/commonSubsystemIds.h @@ -12,6 +12,7 @@ enum: uint8_t { HEATER_HANDLER = 109, SA_DEPL_HANDLER = 110, PLOC_HANDLER = 111, + IMTQ_HANDLER = 112, COMMON_SUBSYSTEM_ID_END }; } diff --git a/fsfw_hal b/fsfw_hal index fe661fff..fce40ebf 160000 --- a/fsfw_hal +++ b/fsfw_hal @@ -1 +1 @@ -Subproject commit fe661fff85e2b8c6149c9603fd68aa3a811bf9bd +Subproject commit fce40ebf9a4a45bafedaee2fc87e5aa10e49fdcc diff --git a/linux/CMakeLists.txt b/linux/CMakeLists.txt index 8314ee97..79891a84 100644 --- a/linux/CMakeLists.txt +++ b/linux/CMakeLists.txt @@ -2,4 +2,5 @@ add_subdirectory(csp) add_subdirectory(utility) add_subdirectory(boardtest) add_subdirectory(devices) -add_subdirectory(fsfwconfig) \ No newline at end of file +add_subdirectory(fsfwconfig) +add_subdirectory(obc) \ No newline at end of file diff --git a/linux/fsfwconfig/OBSWConfig.h.in b/linux/fsfwconfig/OBSWConfig.h.in index 82a57e39..b3002cf4 100644 --- a/linux/fsfwconfig/OBSWConfig.h.in +++ b/linux/fsfwconfig/OBSWConfig.h.in @@ -22,9 +22,11 @@ debugging. */ #define OBSW_ADD_TEST_PST 1 #define TEST_LIBGPIOD 0 -#define TEST_RADIATION_SENSOR_HANDLER 1 +#define TEST_RADIATION_SENSOR_HANDLER 0 #define TEST_SUS_HANDLER 1 #define TEST_PLOC_HANDLER 0 +#define TEST_CCSDS_BRIDGE 0 +#define PERFORM_PTME_TEST 0 #define TE0720 0 #define TE0720_HEATER_TEST 0 @@ -39,6 +41,8 @@ debugging. */ #define L3GD20_GYRO_DEBUG 0 #define DEBUG_RAD_SENSOR 1 #define DEBUG_SUS 1 +#define DEBUG_RTD 1 +#define IMTQ_DEBUG 1 // Leave at one as the BSP is linux. Used by the ADIS16507 device handler #define OBSW_ADIS16507_LINUX_COM_IF 1 diff --git a/linux/fsfwconfig/devices/gpioIds.h b/linux/fsfwconfig/devices/gpioIds.h index 3221c613..102b04b9 100644 --- a/linux/fsfwconfig/devices/gpioIds.h +++ b/linux/fsfwconfig/devices/gpioIds.h @@ -65,7 +65,10 @@ namespace gpioIds { SPI_MUX_BIT_5, SPI_MUX_BIT_6, - CS_RAD_SENSOR + CS_RAD_SENSOR, + + PAPB_BUSY_N, + PAPB_EMPTY }; } diff --git a/linux/fsfwconfig/objects/systemObjectList.h b/linux/fsfwconfig/objects/systemObjectList.h index 8558e335..5d5cdee1 100644 --- a/linux/fsfwconfig/objects/systemObjectList.h +++ b/linux/fsfwconfig/objects/systemObjectList.h @@ -12,6 +12,8 @@ namespace objects { FW_ADDRESS_START = PUS_SERVICE_1_VERIFICATION, FW_ADDRESS_END = TIME_STAMPER, + CCSDS_IP_CORE_BRIDGE = 0x50000500, + PUS_SERVICE_6 = 0x51000500, TM_FUNNEL = 0x52000002, diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index 148cef2b..60059d3c 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -145,11 +145,11 @@ ReturnValue_t pst::pollingSequenceInitDefault(FixedTimeslotTaskIF *thisSequence) /* Radiation sensor */ - thisSequence->addSlot(objects::RAD_SENSOR, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RAD_SENSOR, length * 0.2, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RAD_SENSOR, length * 0.4, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RAD_SENSOR, length * 0.6, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RAD_SENSOR, length * 0.8, DeviceHandlerIF::GET_READ); +// thisSequence->addSlot(objects::RAD_SENSOR, length * 0, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::RAD_SENSOR, length * 0.2, DeviceHandlerIF::SEND_WRITE); +// thisSequence->addSlot(objects::RAD_SENSOR, length * 0.4, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::RAD_SENSOR, length * 0.6, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::RAD_SENSOR, length * 0.8, DeviceHandlerIF::GET_READ); if (length != 3000) { sif::warning << "pollingSequenceInitDefault: Frequency changed. Make sure timing critical " @@ -166,251 +166,251 @@ ReturnValue_t pst::pollingSequenceInitDefault(FixedTimeslotTaskIF *thisSequence) */ /* Write setup */ - thisSequence->addSlot(objects::SUS_1, length * 0.9, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_1, length * 0.9, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_1, length * 0.9, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_1, length * 0.9, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_1, length * 0.9, DeviceHandlerIF::GET_READ); - /* Write setup */ - thisSequence->addSlot(objects::SUS_1, length * 0.901, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_1, length * 0.901, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_1, length * 0.901, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_1, length * 0.901, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_1, length * 0.901, DeviceHandlerIF::GET_READ); - /* Write setup */ - thisSequence->addSlot(objects::SUS_1, length * 0.902, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_1, length * 0.902, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_1, length * 0.902, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_1, length * 0.902, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_1, length * 0.902, DeviceHandlerIF::GET_READ); +// thisSequence->addSlot(objects::SUS_1, length * 0.9, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_1, length * 0.9, SusHandler::FIRST_WRITE); +// thisSequence->addSlot(objects::SUS_1, length * 0.9, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_1, length * 0.9, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_1, length * 0.9, DeviceHandlerIF::GET_READ); +// /* Write setup */ +// thisSequence->addSlot(objects::SUS_1, length * 0.901, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_1, length * 0.901, SusHandler::FIRST_WRITE); +// thisSequence->addSlot(objects::SUS_1, length * 0.901, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_1, length * 0.901, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_1, length * 0.901, DeviceHandlerIF::GET_READ); +// /* Write setup */ +// thisSequence->addSlot(objects::SUS_1, length * 0.902, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_1, length * 0.902, SusHandler::FIRST_WRITE); +// thisSequence->addSlot(objects::SUS_1, length * 0.902, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_1, length * 0.902, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_1, length * 0.902, DeviceHandlerIF::GET_READ); /* Write setup */ - thisSequence->addSlot(objects::SUS_2, length * 0.903, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_2, length * 0.903, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_2, length * 0.903, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_2, length * 0.903, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_2, length * 0.903, DeviceHandlerIF::GET_READ); - /* Write setup */ - thisSequence->addSlot(objects::SUS_2, length * 0.904, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_2, length * 0.904, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_2, length * 0.904, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_2, length * 0.904, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_2, length * 0.904, DeviceHandlerIF::GET_READ); - /* Write setup */ - thisSequence->addSlot(objects::SUS_2, length * 0.905, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_2, length * 0.905, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_2, length * 0.905, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_2, length * 0.905, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_2, length * 0.905, DeviceHandlerIF::GET_READ); +// thisSequence->addSlot(objects::SUS_2, length * 0.903, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_2, length * 0.903, SusHandler::FIRST_WRITE); +// thisSequence->addSlot(objects::SUS_2, length * 0.903, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_2, length * 0.903, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_2, length * 0.903, DeviceHandlerIF::GET_READ); +// /* Write setup */ +// thisSequence->addSlot(objects::SUS_2, length * 0.904, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_2, length * 0.904, SusHandler::SEND_WRITE); +// thisSequence->addSlot(objects::SUS_2, length * 0.904, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_2, length * 0.904, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_2, length * 0.904, DeviceHandlerIF::GET_READ); +// /* Write setup */ +// thisSequence->addSlot(objects::SUS_2, length * 0.905, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_2, length * 0.905, SusHandler::SEND_WRITE); +// thisSequence->addSlot(objects::SUS_2, length * 0.905, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_2, length * 0.905, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_2, length * 0.905, DeviceHandlerIF::GET_READ); /* Write setup */ - thisSequence->addSlot(objects::SUS_3, length * 0.906, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_3, length * 0.906, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_3, length * 0.906, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_3, length * 0.906, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_3, length * 0.906, DeviceHandlerIF::GET_READ); - /* Write setup */ - thisSequence->addSlot(objects::SUS_3, length * 0.907, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_3, length * 0.907, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_3, length * 0.907, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_3, length * 0.907, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_3, length * 0.907, DeviceHandlerIF::GET_READ); - /* Write setup */ - thisSequence->addSlot(objects::SUS_3, length * 0.908, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_3, length * 0.908, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_3, length * 0.908, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_3, length * 0.908, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_3, length * 0.908, DeviceHandlerIF::GET_READ); - - /* Write setup */ - thisSequence->addSlot(objects::SUS_4, length * 0.909, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_4, length * 0.909, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_4, length * 0.909, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_4, length * 0.909, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_4, length * 0.909, DeviceHandlerIF::GET_READ); - /* Write setup */ - thisSequence->addSlot(objects::SUS_4, length * 0.91, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_4, length * 0.91, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_4, length * 0.91, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_4, length * 0.91, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_4, length * 0.91, DeviceHandlerIF::GET_READ); - /* Write setup */ - thisSequence->addSlot(objects::SUS_4, length * 0.911, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_4, length * 0.911, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_4, length * 0.911, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_4, length * 0.911, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_4, length * 0.911, DeviceHandlerIF::GET_READ); - - /* Write setup */ - thisSequence->addSlot(objects::SUS_5, length * 0.912, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_5, length * 0.912, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_5, length * 0.912, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_5, length * 0.912, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_5, length * 0.912, DeviceHandlerIF::GET_READ); - /* Write setup */ - thisSequence->addSlot(objects::SUS_5, length * 0.913, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_5, length * 0.913, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_5, length * 0.913, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_5, length * 0.913, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_5, length * 0.913, DeviceHandlerIF::GET_READ); - /* Write setup */ - thisSequence->addSlot(objects::SUS_5, length * 0.914, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_5, length * 0.914, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_5, length * 0.914, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_5, length * 0.914, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_5, length * 0.914, DeviceHandlerIF::GET_READ); - - /* Write setup */ - thisSequence->addSlot(objects::SUS_6, length * 0.915, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_6, length * 0.915, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_6, length * 0.915, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_6, length * 0.915, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_6, length * 0.915, DeviceHandlerIF::GET_READ); - /* Start ADC conversions */ - thisSequence->addSlot(objects::SUS_6, length * 0.916, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_6, length * 0.916, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_6, length * 0.916, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_6, length * 0.916, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_6, length * 0.916, DeviceHandlerIF::GET_READ); - /* Read ADC conversions from inernal FIFO */ - thisSequence->addSlot(objects::SUS_6, length * 0.917, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_6, length * 0.917, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_6, length * 0.917, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_6, length * 0.917, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_6, length * 0.917, DeviceHandlerIF::GET_READ); - - /* Write setup */ - thisSequence->addSlot(objects::SUS_7, length * 0.918, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_7, length * 0.918, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_7, length * 0.918, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_7, length * 0.918, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_7, length * 0.918, DeviceHandlerIF::GET_READ); - /* Start ADC conversions */ - thisSequence->addSlot(objects::SUS_7, length * 0.919, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_7, length * 0.919, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_7, length * 0.919, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_7, length * 0.919, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_7, length * 0.919, DeviceHandlerIF::GET_READ); - /* Read ADC conversions from inernal FIFO */ - thisSequence->addSlot(objects::SUS_7, length * 0.92, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_7, length * 0.92, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_7, length * 0.92, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_7, length * 0.92, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_7, length * 0.92, DeviceHandlerIF::GET_READ); - - /* Write setup */ - thisSequence->addSlot(objects::SUS_8, length * 0.921, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_8, length * 0.921, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_8, length * 0.921, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_8, length * 0.921, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_8, length * 0.921, DeviceHandlerIF::GET_READ); - /* Start ADC conversions */ - thisSequence->addSlot(objects::SUS_8, length * 0.922, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_8, length * 0.922, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_8, length * 0.922, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_8, length * 0.922, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_8, length * 0.922, DeviceHandlerIF::GET_READ); - /* Read ADC conversions from inernal FIFO */ - thisSequence->addSlot(objects::SUS_8, length * 0.923, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_8, length * 0.923, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_8, length * 0.923, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_8, length * 0.923, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_8, length * 0.923, DeviceHandlerIF::GET_READ); - - /* Write setup */ - thisSequence->addSlot(objects::SUS_9, length * 0.924, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_9, length * 0.924, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_9, length * 0.924, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_9, length * 0.924, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_9, length * 0.924, DeviceHandlerIF::GET_READ); - /* Start ADC conversions */ - thisSequence->addSlot(objects::SUS_9, length * 0.925, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_9, length * 0.925, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_9, length * 0.925, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_9, length * 0.925, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_9, length * 0.925, DeviceHandlerIF::GET_READ); - /* Read ADC conversions */ - thisSequence->addSlot(objects::SUS_9, length * 0.926, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_9, length * 0.926, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_9, length * 0.926, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_9, length * 0.926, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_9, length * 0.926, DeviceHandlerIF::GET_READ); - - /* Write setup */ - thisSequence->addSlot(objects::SUS_10, length * 0.927, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_10, length * 0.927, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_10, length * 0.927, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_10, length * 0.927, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_10, length * 0.927, DeviceHandlerIF::GET_READ); - /* Start ADC conversions */ - thisSequence->addSlot(objects::SUS_10, length * 0.928, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_10, length * 0.928, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_10, length * 0.928, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_10, length * 0.928, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_10, length * 0.928, DeviceHandlerIF::GET_READ); - /* Read ADC conversions */ - thisSequence->addSlot(objects::SUS_10, length * 0.929, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_10, length * 0.929, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_10, length * 0.929, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_10, length * 0.929, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_10, length * 0.929, DeviceHandlerIF::GET_READ); - - /* Write setup */ - thisSequence->addSlot(objects::SUS_11, length * 0.93, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_11, length * 0.93, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_11, length * 0.93, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_11, length * 0.93, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_11, length * 0.93, DeviceHandlerIF::GET_READ); - /* Start ADC conversions */ - thisSequence->addSlot(objects::SUS_11, length * 0.931, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_11, length * 0.931, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_11, length * 0.931, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_11, length * 0.931, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_11, length * 0.931, DeviceHandlerIF::GET_READ); - /* Read ADC conversions */ - thisSequence->addSlot(objects::SUS_11, length * 0.932, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_11, length * 0.932, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_11, length * 0.932, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_11, length * 0.932, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_11, length * 0.932, DeviceHandlerIF::GET_READ); - - /* Write setup */ - thisSequence->addSlot(objects::SUS_12, length * 0.933, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_12, length * 0.933, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_12, length * 0.933, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_12, length * 0.933, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_12, length * 0.933, DeviceHandlerIF::GET_READ); - /* Start ADC conversions */ - thisSequence->addSlot(objects::SUS_12, length * 0.934, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_12, length * 0.934, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_12, length * 0.934, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_12, length * 0.934, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_12, length * 0.934, DeviceHandlerIF::GET_READ); - /* Read ADC conversions */ - thisSequence->addSlot(objects::SUS_12, length * 0.935, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_12, length * 0.935, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_12, length * 0.935, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_12, length * 0.935, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_12, length * 0.935, DeviceHandlerIF::GET_READ); - - /* Write setup */ - thisSequence->addSlot(objects::SUS_13, length * 0.936, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_13, length * 0.936, SusHandler::FIRST_WRITE); - thisSequence->addSlot(objects::SUS_13, length * 0.936, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_13, length * 0.936, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_13, length * 0.936, DeviceHandlerIF::GET_READ); - /* Start ADC conversions */ - thisSequence->addSlot(objects::SUS_13, length * 0.937, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_13, length * 0.937, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_13, length * 0.937, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_13, length * 0.937, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_13, length * 0.937, DeviceHandlerIF::GET_READ); - /* Read ADC conversions */ - thisSequence->addSlot(objects::SUS_13, length * 0.938, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_13, length * 0.938, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_13, length * 0.938, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_13, length * 0.938, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_13, length * 0.938, DeviceHandlerIF::GET_READ); +// thisSequence->addSlot(objects::SUS_3, length * 0.8, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_3, length * 0.8, SusHandler::FIRST_WRITE); +// thisSequence->addSlot(objects::SUS_3, length * 0.8, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_3, length * 0.8, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_3, length * 0.8, DeviceHandlerIF::GET_READ); +// /* Write setup */ +// thisSequence->addSlot(objects::SUS_3, length * 0.91, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_3, length * 0.91, SusHandler::SEND_WRITE); +// thisSequence->addSlot(objects::SUS_3, length * 0.91, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_3, length * 0.91, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_3, length * 0.91, DeviceHandlerIF::GET_READ); +// /* Write setup */ +// thisSequence->addSlot(objects::SUS_3, length * 0.93, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_3, length * 0.93, SusHandler::SEND_WRITE); +// thisSequence->addSlot(objects::SUS_3, length * 0.93, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_3, length * 0.93, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_3, length * 0.93, DeviceHandlerIF::GET_READ); +// +// /* Write setup */ +// thisSequence->addSlot(objects::SUS_4, length * 0.909, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_4, length * 0.909, SusHandler::FIRST_WRITE); +// thisSequence->addSlot(objects::SUS_4, length * 0.909, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_4, length * 0.909, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_4, length * 0.909, DeviceHandlerIF::GET_READ); +// /* Write setup */ +// thisSequence->addSlot(objects::SUS_4, length * 0.91, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_4, length * 0.91, SusHandler::FIRST_WRITE); +// thisSequence->addSlot(objects::SUS_4, length * 0.91, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_4, length * 0.91, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_4, length * 0.91, DeviceHandlerIF::GET_READ); +// /* Write setup */ +// thisSequence->addSlot(objects::SUS_4, length * 0.911, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_4, length * 0.911, SusHandler::FIRST_WRITE); +// thisSequence->addSlot(objects::SUS_4, length * 0.911, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_4, length * 0.911, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_4, length * 0.911, DeviceHandlerIF::GET_READ); +// +// /* Write setup */ +// thisSequence->addSlot(objects::SUS_5, length * 0.912, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_5, length * 0.912, SusHandler::FIRST_WRITE); +// thisSequence->addSlot(objects::SUS_5, length * 0.912, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_5, length * 0.912, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_5, length * 0.912, DeviceHandlerIF::GET_READ); +// /* Write setup */ +// thisSequence->addSlot(objects::SUS_5, length * 0.913, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_5, length * 0.913, SusHandler::FIRST_WRITE); +// thisSequence->addSlot(objects::SUS_5, length * 0.913, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_5, length * 0.913, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_5, length * 0.913, DeviceHandlerIF::GET_READ); +// /* Write setup */ +// thisSequence->addSlot(objects::SUS_5, length * 0.914, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_5, length * 0.914, SusHandler::FIRST_WRITE); +// thisSequence->addSlot(objects::SUS_5, length * 0.914, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_5, length * 0.914, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_5, length * 0.914, DeviceHandlerIF::GET_READ); +// +// /* Write setup */ +// thisSequence->addSlot(objects::SUS_6, length * 0.915, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_6, length * 0.915, SusHandler::FIRST_WRITE); +// thisSequence->addSlot(objects::SUS_6, length * 0.915, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_6, length * 0.915, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_6, length * 0.915, DeviceHandlerIF::GET_READ); +// /* Start ADC conversions */ +// thisSequence->addSlot(objects::SUS_6, length * 0.916, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_6, length * 0.916, DeviceHandlerIF::SEND_WRITE); +// thisSequence->addSlot(objects::SUS_6, length * 0.916, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_6, length * 0.916, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_6, length * 0.916, DeviceHandlerIF::GET_READ); +// /* Read ADC conversions from inernal FIFO */ +// thisSequence->addSlot(objects::SUS_6, length * 0.917, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_6, length * 0.917, DeviceHandlerIF::SEND_WRITE); +// thisSequence->addSlot(objects::SUS_6, length * 0.917, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_6, length * 0.917, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_6, length * 0.917, DeviceHandlerIF::GET_READ); +// +// /* Write setup */ +// thisSequence->addSlot(objects::SUS_7, length * 0.918, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_7, length * 0.918, SusHandler::FIRST_WRITE); +// thisSequence->addSlot(objects::SUS_7, length * 0.918, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_7, length * 0.918, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_7, length * 0.918, DeviceHandlerIF::GET_READ); +// /* Start ADC conversions */ +// thisSequence->addSlot(objects::SUS_7, length * 0.919, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_7, length * 0.919, DeviceHandlerIF::SEND_WRITE); +// thisSequence->addSlot(objects::SUS_7, length * 0.919, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_7, length * 0.919, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_7, length * 0.919, DeviceHandlerIF::GET_READ); +// /* Read ADC conversions from inernal FIFO */ +// thisSequence->addSlot(objects::SUS_7, length * 0.92, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_7, length * 0.92, DeviceHandlerIF::SEND_WRITE); +// thisSequence->addSlot(objects::SUS_7, length * 0.92, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_7, length * 0.92, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_7, length * 0.92, DeviceHandlerIF::GET_READ); +// +// /* Write setup */ +// thisSequence->addSlot(objects::SUS_8, length * 0.921, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_8, length * 0.921, SusHandler::FIRST_WRITE); +// thisSequence->addSlot(objects::SUS_8, length * 0.921, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_8, length * 0.921, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_8, length * 0.921, DeviceHandlerIF::GET_READ); +// /* Start ADC conversions */ +// thisSequence->addSlot(objects::SUS_8, length * 0.922, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_8, length * 0.922, DeviceHandlerIF::SEND_WRITE); +// thisSequence->addSlot(objects::SUS_8, length * 0.922, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_8, length * 0.922, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_8, length * 0.922, DeviceHandlerIF::GET_READ); +// /* Read ADC conversions from inernal FIFO */ +// thisSequence->addSlot(objects::SUS_8, length * 0.923, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_8, length * 0.923, DeviceHandlerIF::SEND_WRITE); +// thisSequence->addSlot(objects::SUS_8, length * 0.923, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_8, length * 0.923, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_8, length * 0.923, DeviceHandlerIF::GET_READ); +// +// /* Write setup */ +// thisSequence->addSlot(objects::SUS_9, length * 0.924, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_9, length * 0.924, SusHandler::FIRST_WRITE); +// thisSequence->addSlot(objects::SUS_9, length * 0.924, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_9, length * 0.924, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_9, length * 0.924, DeviceHandlerIF::GET_READ); +// /* Start ADC conversions */ +// thisSequence->addSlot(objects::SUS_9, length * 0.925, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_9, length * 0.925, DeviceHandlerIF::SEND_WRITE); +// thisSequence->addSlot(objects::SUS_9, length * 0.925, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_9, length * 0.925, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_9, length * 0.925, DeviceHandlerIF::GET_READ); +// /* Read ADC conversions */ +// thisSequence->addSlot(objects::SUS_9, length * 0.926, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_9, length * 0.926, DeviceHandlerIF::SEND_WRITE); +// thisSequence->addSlot(objects::SUS_9, length * 0.926, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_9, length * 0.926, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_9, length * 0.926, DeviceHandlerIF::GET_READ); +// +// /* Write setup */ +// thisSequence->addSlot(objects::SUS_10, length * 0.927, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_10, length * 0.927, SusHandler::FIRST_WRITE); +// thisSequence->addSlot(objects::SUS_10, length * 0.927, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_10, length * 0.927, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_10, length * 0.927, DeviceHandlerIF::GET_READ); +// /* Start ADC conversions */ +// thisSequence->addSlot(objects::SUS_10, length * 0.928, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_10, length * 0.928, DeviceHandlerIF::SEND_WRITE); +// thisSequence->addSlot(objects::SUS_10, length * 0.928, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_10, length * 0.928, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_10, length * 0.928, DeviceHandlerIF::GET_READ); +// /* Read ADC conversions */ +// thisSequence->addSlot(objects::SUS_10, length * 0.929, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_10, length * 0.929, DeviceHandlerIF::SEND_WRITE); +// thisSequence->addSlot(objects::SUS_10, length * 0.929, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_10, length * 0.929, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_10, length * 0.929, DeviceHandlerIF::GET_READ); +// +// /* Write setup */ +// thisSequence->addSlot(objects::SUS_11, length * 0.93, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_11, length * 0.93, SusHandler::FIRST_WRITE); +// thisSequence->addSlot(objects::SUS_11, length * 0.93, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_11, length * 0.93, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_11, length * 0.93, DeviceHandlerIF::GET_READ); +// /* Start ADC conversions */ +// thisSequence->addSlot(objects::SUS_11, length * 0.931, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_11, length * 0.931, DeviceHandlerIF::SEND_WRITE); +// thisSequence->addSlot(objects::SUS_11, length * 0.931, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_11, length * 0.931, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_11, length * 0.931, DeviceHandlerIF::GET_READ); +// /* Read ADC conversions */ +// thisSequence->addSlot(objects::SUS_11, length * 0.932, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_11, length * 0.932, DeviceHandlerIF::SEND_WRITE); +// thisSequence->addSlot(objects::SUS_11, length * 0.932, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_11, length * 0.932, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_11, length * 0.932, DeviceHandlerIF::GET_READ); +// +// /* Write setup */ +// thisSequence->addSlot(objects::SUS_12, length * 0.933, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_12, length * 0.933, SusHandler::FIRST_WRITE); +// thisSequence->addSlot(objects::SUS_12, length * 0.933, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_12, length * 0.933, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_12, length * 0.933, DeviceHandlerIF::GET_READ); +// /* Start ADC conversions */ +// thisSequence->addSlot(objects::SUS_12, length * 0.934, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_12, length * 0.934, DeviceHandlerIF::SEND_WRITE); +// thisSequence->addSlot(objects::SUS_12, length * 0.934, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_12, length * 0.934, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_12, length * 0.934, DeviceHandlerIF::GET_READ); +// /* Read ADC conversions */ +// thisSequence->addSlot(objects::SUS_12, length * 0.935, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_12, length * 0.935, DeviceHandlerIF::SEND_WRITE); +// thisSequence->addSlot(objects::SUS_12, length * 0.935, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_12, length * 0.935, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_12, length * 0.935, DeviceHandlerIF::GET_READ); +// +// /* Write setup */ +// thisSequence->addSlot(objects::SUS_13, length * 0.936, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_13, length * 0.936, SusHandler::FIRST_WRITE); +// thisSequence->addSlot(objects::SUS_13, length * 0.936, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_13, length * 0.936, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_13, length * 0.936, DeviceHandlerIF::GET_READ); +// /* Start ADC conversions */ +// thisSequence->addSlot(objects::SUS_13, length * 0.937, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_13, length * 0.937, DeviceHandlerIF::SEND_WRITE); +// thisSequence->addSlot(objects::SUS_13, length * 0.937, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_13, length * 0.937, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_13, length * 0.937, DeviceHandlerIF::GET_READ); +// /* Read ADC conversions */ +// thisSequence->addSlot(objects::SUS_13, length * 0.938, DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::SUS_13, length * 0.938, DeviceHandlerIF::SEND_WRITE); +// thisSequence->addSlot(objects::SUS_13, length * 0.938, DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::SUS_13, length * 0.938, DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::SUS_13, length * 0.938, DeviceHandlerIF::GET_READ); if (thisSequence->checkSequence() == HasReturnvaluesIF::RETURN_OK) { return HasReturnvaluesIF::RETURN_OK; diff --git a/linux/fsfwconfig/returnvalues/classIds.h b/linux/fsfwconfig/returnvalues/classIds.h index 6282980e..d1bfe7e2 100644 --- a/linux/fsfwconfig/returnvalues/classIds.h +++ b/linux/fsfwconfig/returnvalues/classIds.h @@ -2,7 +2,7 @@ #define FSFWCONFIG_RETURNVALUES_CLASSIDS_H_ #include -#include "commonClassIds.h" +#include /** * Source IDs starts at 73 for now diff --git a/linux/obc/CCSDSIPCoreBridge.cpp b/linux/obc/CCSDSIPCoreBridge.cpp new file mode 100644 index 00000000..2678e76f --- /dev/null +++ b/linux/obc/CCSDSIPCoreBridge.cpp @@ -0,0 +1,136 @@ +#include +#include + +#include + +CCSDSIPCoreBridge::CCSDSIPCoreBridge(object_id_t objectId, object_id_t tcDestination, + object_id_t tmStoreId, object_id_t tcStoreId, LinuxLibgpioIF* gpioComIF, + std::string uioPtme, gpioId_t papbBusyId, gpioId_t papbEmptyId) : + TmTcBridge(objectId, tcDestination, tmStoreId, tcStoreId), gpioComIF(gpioComIF), uioPtme( + uioPtme), papbBusyId(papbBusyId), papbEmptyId(papbEmptyId) { +} + +CCSDSIPCoreBridge::~CCSDSIPCoreBridge() { +} + +ReturnValue_t CCSDSIPCoreBridge::initialize() { + ReturnValue_t result = TmTcBridge::initialize(); + + fd = open("/dev/uio0", O_RDWR); + if (fd < 1) { + sif::debug << "CCSDSIPCoreBridge::initialize: Invalid UIO device file" << std::endl; + return RETURN_FAILED; + } + + /** + * Map uio device in virtual address space + * PROT_WRITE: Map uio device in writable only mode + */ + ptmeBaseAddress = static_cast(mmap(NULL, MAP_SIZE, PROT_READ | PROT_WRITE, + MAP_SHARED, fd, 0)); + + if (ptmeBaseAddress == MAP_FAILED) { + sif::error << "CCSDSIPCoreBridge::initialize: Failed to map uio address" << std::endl; + return RETURN_FAILED; + } + + return result; +} + +ReturnValue_t CCSDSIPCoreBridge::handleTm() { + +#if PERFORM_PTME_TEST == 1 + return sendTestFrame(); +#else + return TmTcBridge::handleTm(); +#endif + +} + +ReturnValue_t CCSDSIPCoreBridge::sendTm(const uint8_t * data, size_t dataLen) { + + if(pollPapbBusySignal() == RETURN_OK) { + startPacketTransfer(); + } + + for(size_t idx = 0; idx < dataLen; idx++) { + if(pollPapbBusySignal() == RETURN_OK) { + *(ptmeBaseAddress + PTME_DATA_REG_OFFSET) = static_cast(*(data + idx)); + } + else { + sif::debug << "CCSDSIPCoreBridge::sendTm: Only written " << idx - 1 << " of " << dataLen + << " data" << std::endl; + return RETURN_FAILED; + } + } + + if(pollPapbBusySignal() == RETURN_OK) { + endPacketTransfer(); + } + return RETURN_OK; +} + +void CCSDSIPCoreBridge::startPacketTransfer() { + *ptmeBaseAddress = PTME_CONFIG_START; +} + +void CCSDSIPCoreBridge::endPacketTransfer() { + *ptmeBaseAddress = PTME_CONFIG_END; +} + +ReturnValue_t CCSDSIPCoreBridge::pollPapbBusySignal() { + int papbBusyState = 0; + ReturnValue_t result = RETURN_OK; + + /** Check if PAPB interface is ready to receive data */ + result = gpioComIF->readGpio(papbBusyId, &papbBusyState); + if (result != RETURN_OK) { + sif::debug << "CCSDSIPCoreBridge::pollPapbBusySignal: Failed to read papb busy signal" + << std::endl; + return RETURN_FAILED; + } + if (!papbBusyState) { + sif::debug << "CCSDSIPCoreBridge::pollPapbBusySignal: PAPB busy" << std::endl; + return PAPB_BUSY; + } + + return RETURN_OK; +} + +void CCSDSIPCoreBridge::isPtmeBufferEmpty() { + ReturnValue_t result = RETURN_OK; + int papbEmptyState = 1; + + result = gpioComIF->readGpio(papbEmptyId, &papbEmptyState); + + if (result != RETURN_OK) { + sif::debug << "CCSDSIPCoreBridge::isPtmeBufferEmpty: Failed to read papb empty signal" + << std::endl; + return; + } + + if (papbEmptyState == 1) { + sif::debug << "CCSDSIPCoreBridge::isPtmeBufferEmpty: Buffer is empty" << std::endl; + } + else { + sif::debug << "CCSDSIPCoreBridge::isPtmeBufferEmpty: Buffer is not empty" << std::endl; + } + return; +} + +ReturnValue_t CCSDSIPCoreBridge::sendTestFrame() { + /** Size of one complete transfer frame data field amounts to 1105 bytes */ + uint8_t testPacket[1105]; + + /** Fill one test packet */ + for(int idx = 0; idx < 1105; idx++) { + testPacket[idx] = static_cast(idx & 0xFF); + } + + ReturnValue_t result = sendTm(testPacket, 1105); + if(result != RETURN_OK) { + return result; + } + + return RETURN_OK; +} diff --git a/linux/obc/CCSDSIPCoreBridge.h b/linux/obc/CCSDSIPCoreBridge.h new file mode 100644 index 00000000..5b7674e9 --- /dev/null +++ b/linux/obc/CCSDSIPCoreBridge.h @@ -0,0 +1,130 @@ +#ifndef MISSION_OBC_CCSDSIPCOREBRIDGE_H_ +#define MISSION_OBC_CCSDSIPCOREBRIDGE_H_ + +#include +#include +#include +#include +#include "OBSWConfig.h" + +/** + * @brief This class handles the interfacing to the telemetry (PTME) and telecommand (PDEC) IP + * cores responsible for the CCSDS encoding and decoding. The IP cores are implemented + * on the programmable logic and are accessible through the linux UIO driver. + */ +class CCSDSIPCoreBridge: public TmTcBridge { +public: + /** + * @brief Constructor + * + * @param objectId + * @param tcDestination + * @param tmStoreId + * @param tcStoreId + * @param uioPtme Name of the uio device file which provides access to the PTME IP Core. + * @param papbBusyId The ID of the GPIO which is connected to the PAPBBusy_N signal of the + * PTME IP Core. A low logic level indicates the PTME is not ready to + * receive more data. + * @param papbEmptyId The ID of the GPIO which is connected to the PAPBEmpty signal of the + * PTME IP Core. The signal is high when there are no packets in the + * external buffer memory (BRAM). + */ + CCSDSIPCoreBridge(object_id_t objectId, object_id_t tcDestination, object_id_t tmStoreId, + object_id_t tcStoreId, LinuxLibgpioIF* gpioComIF, std::string uioPtme, + gpioId_t papbBusyId, gpioId_t papbEmptyId); + virtual ~CCSDSIPCoreBridge(); + + ReturnValue_t initialize() override; + +protected: + + /** + * Overwriting this function to provide the capability of testing the PTME IP Core + * implementation. + */ + virtual ReturnValue_t handleTm() override; + + virtual ReturnValue_t sendTm(const uint8_t * data, size_t dataLen) override; + +private: + + static const uint8_t INTERFACE_ID = CLASS_ID::CCSDS_IP_CORE_BRIDGE; + + static const ReturnValue_t PAPB_BUSY = MAKE_RETURN_CODE(0xA0); + + + /** Size of mapped address space. 4k (minimal size of pl device) */ +// static const int MAP_SIZE = 0xFA0; + static const int MAP_SIZE = 0x1000; + + /** + * Configuration bits: + * bit[1:0]: Size of data (1,2,3 or 4 bytes). 1 Byte <=> b00 + * bit[2]: Set this bit to 1 to abort a transfered packet + * bit[3]: Signals to PTME the start of a new telemetry packet + */ + static const uint32_t PTME_CONFIG_START = 0x8; + + /** + * Writing this word to the ptme base address signals to the PTME that a complete tm packet has + * been transferred. + */ + static const uint32_t PTME_CONFIG_END = 0x0; + + /** + * Writing to this offset within the PTME memory space will insert data for encoding to the + * PTME IP core. + * The address offset is 0x400 (= 4 * 256) + */ + static const int PTME_DATA_REG_OFFSET = 256; + + LinuxLibgpioIF* gpioComIF = nullptr; + + /** The uio device file related to the PTME IP Core */ + std::string uioPtme; + + /** Pulled to low when PTME not ready to receive data */ + gpioId_t papbBusyId = gpio::NO_GPIO; + + /** High when externally buffer memory of PTME is empty */ + gpioId_t papbEmptyId = gpio::NO_GPIO; + + /** The file descriptor of the UIO driver */ + int fd; + + uint32_t* ptmeBaseAddress = nullptr; + + /** + * @brief This function sends the config byte to the PTME IP Core to initiate a packet + * transfer. + */ + void startPacketTransfer(); + + /** + * @brief This function sends the config byte to the PTME IP Core to signal the end of a + * packet transfer. + */ + void endPacketTransfer(); + + /** + * @brief This function reads the papb busy signal indicating whether the PAPB interface is + * ready to receive more data or not. PAPB is ready when PAPB_Busy_N == '1'. + * + * @return RETURN_OK when ready to receive data else PAPB_BUSY. + */ + ReturnValue_t pollPapbBusySignal(); + + /** + * @brief This function can be used for debugging to check wheter there are packets in + * the packet buffer of the PTME or not. + */ + void isPtmeBufferEmpty(); + + /** + * @brief This function sends a complete telemetry transfer frame data field (1105 bytes) + * to the input of the PTME IP Core. Can be used to test the implementation. + */ + ReturnValue_t sendTestFrame(); +}; + +#endif /* MISSION_OBC_CCSDSIPCOREBRIDGE_H_ */ diff --git a/linux/obc/CMakeLists.txt b/linux/obc/CMakeLists.txt new file mode 100644 index 00000000..79d9ba9b --- /dev/null +++ b/linux/obc/CMakeLists.txt @@ -0,0 +1,5 @@ +target_sources(${TARGET_NAME} PUBLIC + CCSDSIPCoreBridge.cpp +) + + diff --git a/mission/devices/IMTQHandler.cpp b/mission/devices/IMTQHandler.cpp index 5751dd50..bd5ed2d2 100644 --- a/mission/devices/IMTQHandler.cpp +++ b/mission/devices/IMTQHandler.cpp @@ -3,11 +3,13 @@ #include #include - +#include IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) : DeviceHandlerBase(objectId, comIF, comCookie), engHkDataset(this), calMtmMeasurementSet( - this), rawMtmMeasurementSet(this) { + this), rawMtmMeasurementSet(this), posXselfTestDataset(this), negXselfTestDataset( + this), posYselfTestDataset(this), negYselfTestDataset(this), posZselfTestDataset( + this), negZselfTestDataset(this) { if (comCookie == NULL) { sif::error << "IMTQHandler: Invalid com cookie" << std::endl; } @@ -16,28 +18,20 @@ IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF * com IMTQHandler::~IMTQHandler() { } - -void IMTQHandler::doStartUp(){ - if(mode == _MODE_START_UP){ - //TODO: Set to MODE_ON again - setMode(MODE_NORMAL); - communicationStep = CommunicationStep::SELF_TEST; - } +void IMTQHandler::doStartUp() { +#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1 + setMode(MODE_NORMAL); +#else + setMode(_MODE_TO_ON); +#endif } -void IMTQHandler::doShutDown(){ +void IMTQHandler::doShutDown() { } -ReturnValue_t IMTQHandler::buildNormalDeviceCommand( - DeviceCommandId_t * id) { +ReturnValue_t IMTQHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) { switch (communicationStep) { - case CommunicationStep::SELF_TEST: -// *id = IMTQ::SELF_TEST; -//TODO: Implementing self test command. On-hold because of issue with humidity in clean -// room - communicationStep = CommunicationStep::GET_ENG_HK_DATA; - break; case CommunicationStep::GET_ENG_HK_DATA: *id = IMTQ::GET_ENG_HK_DATA; communicationStep = CommunicationStep::START_MTM_MEASUREMENT; @@ -56,22 +50,68 @@ ReturnValue_t IMTQHandler::buildNormalDeviceCommand( break; default: sif::debug << "IMTQHandler::buildNormalDeviceCommand: Invalid communication step" - << std::endl; + << std::endl; break; } return buildCommandFromCommand(*id, NULL, 0); } -ReturnValue_t IMTQHandler::buildTransitionDeviceCommand( - DeviceCommandId_t * id){ - return HasReturnvaluesIF::RETURN_OK; +ReturnValue_t IMTQHandler::buildTransitionDeviceCommand(DeviceCommandId_t * id) { + return RETURN_OK; } -ReturnValue_t IMTQHandler::buildCommandFromCommand( - DeviceCommandId_t deviceCommand, const uint8_t * commandData, - size_t commandDataLen) { - switch(deviceCommand) { - case(IMTQ::START_ACTUATION_DIPOLE): { +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::NEG_X_SELF_TEST): { + commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; + commandBuffer[1] = IMTQ::SELF_TEST_AXIS::X_NEGATIVE; + rawPacket = commandBuffer; + rawPacketLen = 2; + return RETURN_OK; + } + case (IMTQ::POS_Y_SELF_TEST): { + commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; + commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Y_POSITIVE; + rawPacket = commandBuffer; + rawPacketLen = 2; + return RETURN_OK; + } + case (IMTQ::NEG_Y_SELF_TEST): { + commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; + commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Y_NEGATIVE; + rawPacket = commandBuffer; + rawPacketLen = 2; + return RETURN_OK; + } + case (IMTQ::POS_Z_SELF_TEST): { + commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; + commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Z_POSITIVE; + rawPacket = commandBuffer; + rawPacketLen = 2; + return RETURN_OK; + } + case (IMTQ::NEG_Z_SELF_TEST): { + commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; + commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Z_NEGATIVE; + rawPacket = commandBuffer; + rawPacketLen = 2; + return RETURN_OK; + } + case (IMTQ::GET_SELF_TEST_RESULT): { + commandBuffer[0] = IMTQ::CC::GET_SELF_TEST_RESULT; + rawPacket = commandBuffer; + rawPacketLen = 1; + return RETURN_OK; + } + case (IMTQ::START_ACTUATION_DIPOLE): { /* IMTQ expects low byte first */ commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE; commandBuffer[1] = *(commandData + 1); @@ -86,43 +126,51 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand( rawPacketLen = 9; return RETURN_OK; } - case(IMTQ::GET_ENG_HK_DATA): { - commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA; - rawPacket = commandBuffer; - rawPacketLen = 1; - return RETURN_OK; - } - case(IMTQ::GET_COMMANDED_DIPOLE): { - commandBuffer[0] = IMTQ::CC::GET_COMMANDED_DIPOLE; - rawPacket = commandBuffer; - rawPacketLen = 1; - return RETURN_OK; - } - case(IMTQ::START_MTM_MEASUREMENT): { - commandBuffer[0] = IMTQ::CC::START_MTM_MEASUREMENT; - rawPacket = commandBuffer; - rawPacketLen = 1; - return RETURN_OK; - } - case(IMTQ::GET_CAL_MTM_MEASUREMENT): { - commandBuffer[0] = IMTQ::CC::GET_CAL_MTM_MEASUREMENT; - rawPacket = commandBuffer; - rawPacketLen = 1; - return RETURN_OK; - } - case(IMTQ::GET_RAW_MTM_MEASUREMENT): { - commandBuffer[0] = IMTQ::CC::GET_RAW_MTM_MEASUREMENT; - rawPacket = commandBuffer; - rawPacketLen = 1; - return RETURN_OK; - } - default: - return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; - } - return HasReturnvaluesIF::RETURN_FAILED; + case (IMTQ::GET_ENG_HK_DATA): { + commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA; + rawPacket = commandBuffer; + rawPacketLen = 1; + return RETURN_OK; + } + case (IMTQ::GET_COMMANDED_DIPOLE): { + commandBuffer[0] = IMTQ::CC::GET_COMMANDED_DIPOLE; + rawPacket = commandBuffer; + rawPacketLen = 1; + return RETURN_OK; + } + case (IMTQ::START_MTM_MEASUREMENT): { + commandBuffer[0] = IMTQ::CC::START_MTM_MEASUREMENT; + rawPacket = commandBuffer; + rawPacketLen = 1; + return RETURN_OK; + } + case (IMTQ::GET_CAL_MTM_MEASUREMENT): { + commandBuffer[0] = IMTQ::CC::GET_CAL_MTM_MEASUREMENT; + rawPacket = commandBuffer; + rawPacketLen = 1; + return RETURN_OK; + } + case (IMTQ::GET_RAW_MTM_MEASUREMENT): { + commandBuffer[0] = IMTQ::CC::GET_RAW_MTM_MEASUREMENT; + rawPacket = commandBuffer; + rawPacketLen = 1; + return RETURN_OK; + } + default: + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } + return HasReturnvaluesIF::RETURN_FAILED; } void IMTQHandler::fillCommandAndReplyMap() { + this->insertInCommandAndReplyMap(IMTQ::POS_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + this->insertInCommandAndReplyMap(IMTQ::NEG_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + this->insertInCommandAndReplyMap(IMTQ::POS_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + this->insertInCommandAndReplyMap(IMTQ::NEG_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + this->insertInCommandAndReplyMap(IMTQ::POS_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + this->insertInCommandAndReplyMap(IMTQ::NEG_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); + this->insertInCommandAndReplyMap(IMTQ::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, @@ -137,47 +185,54 @@ void IMTQHandler::fillCommandAndReplyMap() { IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT); } -ReturnValue_t IMTQHandler::scanForReply(const uint8_t *start, - size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) { +ReturnValue_t IMTQHandler::scanForReply(const uint8_t *start, size_t remainingSize, + DeviceCommandId_t *foundId, size_t *foundLen) { ReturnValue_t result = RETURN_OK; - switch(*start) { - case(IMTQ::CC::START_ACTUATION_DIPOLE): + switch (*start) { + case (IMTQ::CC::START_ACTUATION_DIPOLE): *foundLen = IMTQ::SIZE_STATUS_REPLY; *foundId = IMTQ::START_ACTUATION_DIPOLE; - break; - case(IMTQ::CC::START_MTM_MEASUREMENT): + break; + case (IMTQ::CC::START_MTM_MEASUREMENT): *foundLen = IMTQ::SIZE_STATUS_REPLY; *foundId = IMTQ::START_MTM_MEASUREMENT; - break; - case(IMTQ::CC::GET_ENG_HK_DATA): + break; + case (IMTQ::CC::GET_ENG_HK_DATA): *foundLen = IMTQ::SIZE_ENG_HK_DATA_REPLY; *foundId = IMTQ::GET_ENG_HK_DATA; - break; - case(IMTQ::CC::GET_COMMANDED_DIPOLE): + break; + case (IMTQ::CC::GET_COMMANDED_DIPOLE): *foundLen = IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY; *foundId = IMTQ::GET_COMMANDED_DIPOLE; - break; - case(IMTQ::CC::GET_CAL_MTM_MEASUREMENT): + break; + case (IMTQ::CC::GET_CAL_MTM_MEASUREMENT): *foundLen = IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT; *foundId = IMTQ::GET_CAL_MTM_MEASUREMENT; - break; - case(IMTQ::CC::GET_RAW_MTM_MEASUREMENT): + break; + case (IMTQ::CC::GET_RAW_MTM_MEASUREMENT): *foundLen = IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT; *foundId = IMTQ::GET_RAW_MTM_MEASUREMENT; - break; - default: + 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; - } + break; + } return result; } -ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, - const uint8_t *packet) { +ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { ReturnValue_t result = RETURN_OK; @@ -188,6 +243,12 @@ ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, } 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 */ @@ -204,21 +265,49 @@ ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, 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 DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; + } + } - return RETURN_OK; + return RETURN_OK; } -void IMTQHandler::setNormalDatapoolEntriesInvalid(){ +void IMTQHandler::setNormalDatapoolEntriesInvalid() { } -uint32_t IMTQHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo){ - return 500; +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 if (sid == negXselfTestDataset.getSid()) { + return &negXselfTestDataset; + } else if (sid == posYselfTestDataset.getSid()) { + return &posYselfTestDataset; + } else if (sid == negYselfTestDataset.getSid()) { + return &negYselfTestDataset; + } else if (sid == posZselfTestDataset.getSid()) { + return &posZselfTestDataset; + } else if (sid == negZselfTestDataset.getSid()) { + return &negZselfTestDataset; + } else { + sif::error << "IMTQHandler::getDataSetHandle: Invalid sid" << std::endl; + return nullptr; + } +} + +uint32_t IMTQHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { + return 5000; } ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, @@ -238,9 +327,9 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat 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::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 */ @@ -249,12 +338,295 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat localDataPoolMap.emplace(IMTQ::MTM_RAW_Z, new PoolEntry( { 0 })); localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry( { 0 })); + /** INIT measurements for positive X axis test */ + localDataPoolMap.emplace(IMTQ::INIT_POS_X_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_X_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_X_RAW_MAG_Y, new PoolEntry( { 0 })); + 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 })); + + /** INIT measurements for negative X axis test */ + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + + /** INIT measurements for positive Y axis test */ + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + + /** INIT measurements for negative Y axis test */ + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + + /** INIT measurements for positive Z axis test */ + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + + /** INIT measurements for negative Z axis test */ + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + + localDataPoolMap.emplace(IMTQ::POS_X_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::POS_X_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::POS_X_RAW_MAG_Y, new PoolEntry( { 0 })); + 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::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 })); + + /** FINA measurements for positive X axis test */ + localDataPoolMap.emplace(IMTQ::FINA_POS_X_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + + /** FINA measurements for negative X axis test */ + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + + /** FINA measurements for positive Y axis test */ + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + + /** FINA measurements for negative Y axis test */ + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + + /** FINA measurements for positive Z axis test */ + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + + /** FINA measurements for negative Z axis test */ + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_ERR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_RAW_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_RAW_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_RAW_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_CAL_MAG_X, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_CAL_MAG_Y, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_CAL_MAG_Z, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_X_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_Y_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_Z_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + return HasReturnvaluesIF::RETURN_OK; } +ReturnValue_t IMTQHandler::getSelfTestCommandId(DeviceCommandId_t* id) { + DeviceCommandId_t commandId = getPendingCommand(); + switch (commandId) { + case IMTQ::POS_X_SELF_TEST: + case IMTQ::NEG_X_SELF_TEST: + case IMTQ::POS_Y_SELF_TEST: + case IMTQ::NEG_Y_SELF_TEST: + case IMTQ::POS_Z_SELF_TEST: + case IMTQ::NEG_Z_SELF_TEST: + *id = commandId; + break; + default: + sif::error << "IMTQHandler::getSelfTestCommandId: Reply does not match to pending " + << "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) { + switch (cmdErrorField) { case 0: return RETURN_OK; case 1: @@ -278,12 +650,13 @@ ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) { return INTERNAL_PROCESSING_ERROR; default: sif::error << "IMTQHandler::parseStatusByte: CMD Error field contains unknown error code " - << cmdErrorField << std::endl; + << cmdErrorField << std::endl; return CMD_ERR_UNKNOWN; } } void IMTQHandler::fillEngHkDataset(const uint8_t* packet) { + PoolReadGuard rg(&engHkDataset); uint8_t offset = 2; engHkDataset.digitalVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; @@ -293,11 +666,14 @@ void IMTQHandler::fillEngHkDataset(const uint8_t* packet) { offset += 2; engHkDataset.analogCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; - engHkDataset.coilXCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; + engHkDataset.coilXCurrentmA = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; offset += 2; - engHkDataset.coilYCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; + engHkDataset.coilYCurrentmA = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; offset += 2; - engHkDataset.coilZCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; + engHkDataset.coilZCurrentmA = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; offset += 2; engHkDataset.coilXTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); offset += 2; @@ -307,7 +683,7 @@ void IMTQHandler::fillEngHkDataset(const uint8_t* packet) { offset += 2; engHkDataset.mcuTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); -#if OBSW_VERBOSE_LEVEL >= 1 && IMQT_DEBUG == 1 +#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; @@ -368,6 +744,7 @@ void IMTQHandler::handleGetCommandedDipoleReply(const uint8_t* packet) { } 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); @@ -380,7 +757,7 @@ void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) { offset += 4; calMtmMeasurementSet.coilActuationStatus = (*(packet + offset + 3) << 24) | (*(packet + offset + 2) << 16) | (*(packet + offset + 1) << 8) | (*(packet + offset)); -#if OBSW_VERBOSE_LEVEL >= 1 && IMQT_DEBUG == 1 +#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" @@ -393,6 +770,7 @@ void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) { } 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; @@ -405,7 +783,7 @@ void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) { offset += 4; rawMtmMeasurementSet.coilActuationStatus = (*(packet + offset + 3) << 24) | (*(packet + offset + 2) << 16) | (*(packet + offset + 1) << 8) | (*(packet + offset)); -#if OBSW_VERBOSE_LEVEL >= 1 && IMQT_DEBUG == 1 +#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" @@ -416,3 +794,1336 @@ void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) { << (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; + } + case IMTQ::SELF_TEST_STEPS::X_NEGATIVE: { + handleNegativeXSelfTestReply(packet); + break; + } + case IMTQ::SELF_TEST_STEPS::Y_POSITIVE: { + handlePositiveYSelfTestReply(packet); + break; + } + case IMTQ::SELF_TEST_STEPS::Y_NEGATIVE: { + handleNegativeYSelfTestReply(packet); + break; + } + case IMTQ::SELF_TEST_STEPS::Z_POSITIVE: { + handlePositiveZSelfTestReply(packet); + break; + } + case IMTQ::SELF_TEST_STEPS::Z_NEGATIVE: { + handleNegativeZSelfTestReply(packet); + break; + } + default: + break; + } +} + +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 = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posXselfTestDataset.initCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posXselfTestDataset.initCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posXselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posXselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posXselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** +X measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + posXselfTestDataset.err = *(packet + offset); + offset += 2; // STEP byte will not be stored + posXselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posXselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posXselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posXselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posXselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posXselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posXselfTestDataset.coilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posXselfTestDataset.coilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posXselfTestDataset.coilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posXselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posXselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posXselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** FINA measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + posXselfTestDataset.finaErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + posXselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posXselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posXselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posXselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posXselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posXselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posXselfTestDataset.finaCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posXselfTestDataset.finaCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posXselfTestDataset.finaCoilZCurrent = static_cast(*(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(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(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(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::handleNegativeXSelfTestReply(const uint8_t* packet) { + PoolReadGuard rg(&posXselfTestDataset); + + uint16_t offset = 2; + /** Init measurements */ + negXselfTestDataset.initErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + negXselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negXselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negXselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negXselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negXselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negXselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negXselfTestDataset.initCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negXselfTestDataset.initCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negXselfTestDataset.initCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negXselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negXselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negXselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** +X measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + negXselfTestDataset.err = *(packet + offset); + offset += 2; // STEP byte will not be stored + negXselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negXselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negXselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negXselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negXselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negXselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negXselfTestDataset.coilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negXselfTestDataset.coilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negXselfTestDataset.coilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negXselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negXselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negXselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** FINA measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + negXselfTestDataset.finaErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + negXselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negXselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negXselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negXselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negXselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negXselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negXselfTestDataset.finaCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negXselfTestDataset.finaCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negXselfTestDataset.finaCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negXselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negXselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negXselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + +#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1 + sif::info << "IMTQ self test (INIT) err: " + << static_cast(negXselfTestDataset.initErr.value) << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field X: " << negXselfTestDataset.initRawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << negXselfTestDataset.initRawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << negXselfTestDataset.initRawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " + << negXselfTestDataset.initCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " + << negXselfTestDataset.initCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " + << negXselfTestDataset.initCalMagZ << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) coil X current: " << negXselfTestDataset.initCoilXCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y current: " << negXselfTestDataset.initCoilYCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z current: " << negXselfTestDataset.initCoilZCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil X temperature: " + << negXselfTestDataset.initCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y temperature: " + << negXselfTestDataset.initCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z temperature: " + << negXselfTestDataset.initCoilZTemperature << " °C" << std::endl; + + sif::info << "IMTQ self test (-X) err: " + << static_cast(negXselfTestDataset.err.value) << std::endl; + sif::info << "IMTQ self test (-X) raw magnetic field X: " << negXselfTestDataset.rawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (-X) raw magnetic field Y: " << negXselfTestDataset.rawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (-X) raw magnetic field Z: " << negXselfTestDataset.rawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (-X) calibrated magnetic field X: " << negXselfTestDataset.calMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (-X) calibrated magnetic field Y: " << negXselfTestDataset.calMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (-X) calibrated magnetic field Z: " << negXselfTestDataset.calMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (-X) coil X current: " << negXselfTestDataset.coilXCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (-X) coil Y current: " << negXselfTestDataset.coilYCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (-X) coil Z current: " << negXselfTestDataset.coilZCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (-X) coil X temperature: " << negXselfTestDataset.coilXTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (-X) coil Y temperature: " << negXselfTestDataset.coilYTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (-X) coil Z temperature: " << negXselfTestDataset.coilZTemperature + << " °C" << std::endl; + + sif::info << "IMTQ self test (FINA) err: " + << static_cast(negXselfTestDataset.finaErr.value) << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field X: " << negXselfTestDataset.finaRawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << negXselfTestDataset.finaRawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << negXselfTestDataset.finaRawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " + << negXselfTestDataset.finaCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " + << negXselfTestDataset.finaCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " + << negXselfTestDataset.finaCalMagZ << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) coil X current: " << negXselfTestDataset.finaCoilXCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y current: " << negXselfTestDataset.finaCoilYCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z current: " << negXselfTestDataset.finaCoilZCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil X temperature: " + << negXselfTestDataset.finaCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y temperature: " + << negXselfTestDataset.finaCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z temperature: " + << negXselfTestDataset.finaCoilZTemperature << " °C" << std::endl; +#endif +} + +void IMTQHandler::handlePositiveYSelfTestReply(const uint8_t* packet) { + PoolReadGuard rg(&posXselfTestDataset); + + uint16_t offset = 2; + /** Init measurements */ + posYselfTestDataset.initErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + posYselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posYselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posYselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posYselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posYselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posYselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posYselfTestDataset.initCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posYselfTestDataset.initCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posYselfTestDataset.initCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posYselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posYselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posYselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** +X measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + posYselfTestDataset.err = *(packet + offset); + offset += 2; // STEP byte will not be stored + posYselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posYselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posYselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posYselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posYselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posYselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posYselfTestDataset.coilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posYselfTestDataset.coilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posYselfTestDataset.coilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posYselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posYselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posYselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** FINA measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + posYselfTestDataset.finaErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + posYselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posYselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posYselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posYselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posYselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posYselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posYselfTestDataset.finaCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posYselfTestDataset.finaCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posYselfTestDataset.finaCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posYselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posYselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posYselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + +#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1 + sif::info << "IMTQ self test (INIT) err: " + << static_cast(posYselfTestDataset.initErr.value) << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field X: " << posYselfTestDataset.initRawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << posYselfTestDataset.initRawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << posYselfTestDataset.initRawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " + << posYselfTestDataset.initCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " + << posYselfTestDataset.initCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " + << posYselfTestDataset.initCalMagZ << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) coil X current: " << posYselfTestDataset.initCoilXCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y current: " << posYselfTestDataset.initCoilYCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z current: " << posYselfTestDataset.initCoilZCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil X temperature: " + << posYselfTestDataset.initCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y temperature: " + << posYselfTestDataset.initCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z temperature: " + << posYselfTestDataset.initCoilZTemperature << " °C" << std::endl; + + sif::info << "IMTQ self test (+Y) err: " + << static_cast(posYselfTestDataset.err.value) << std::endl; + sif::info << "IMTQ self test (+Y) raw magnetic field X: " << posYselfTestDataset.rawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (+Y) raw magnetic field Y: " << posYselfTestDataset.rawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (+Y) raw magnetic field Z: " << posYselfTestDataset.rawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (+Y) calibrated magnetic field X: " << posYselfTestDataset.calMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (+Y) calibrated magnetic field Y: " << posYselfTestDataset.calMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (+Y) calibrated magnetic field Z: " << posYselfTestDataset.calMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (+Y) coil X current: " << posYselfTestDataset.coilXCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (+Y) coil Y current: " << posYselfTestDataset.coilYCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (+Y) coil Z current: " << posYselfTestDataset.coilZCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (+Y) coil X temperature: " << posYselfTestDataset.coilXTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (+Y) coil Y temperature: " << posYselfTestDataset.coilYTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (+Y) coil Z temperature: " << posYselfTestDataset.coilZTemperature + << " °C" << std::endl; + + sif::info << "IMTQ self test (FINA) err: " + << static_cast(posYselfTestDataset.finaErr.value) << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field X: " << posYselfTestDataset.finaRawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << posYselfTestDataset.finaRawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << posYselfTestDataset.finaRawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " + << posYselfTestDataset.finaCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " + << posYselfTestDataset.finaCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " + << posYselfTestDataset.finaCalMagZ << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) coil X current: " << posYselfTestDataset.finaCoilXCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y current: " << posYselfTestDataset.finaCoilYCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z current: " << posYselfTestDataset.finaCoilZCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil X temperature: " + << posYselfTestDataset.finaCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y temperature: " + << posYselfTestDataset.finaCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z temperature: " + << posYselfTestDataset.finaCoilZTemperature << " °C" << std::endl; +#endif +} + +void IMTQHandler::handleNegativeYSelfTestReply(const uint8_t* packet) { + PoolReadGuard rg(&posXselfTestDataset); + + uint16_t offset = 2; + /** Init measurements */ + posZselfTestDataset.initErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + negYselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negYselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negYselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negYselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negYselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negYselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negYselfTestDataset.initCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negYselfTestDataset.initCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negYselfTestDataset.initCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negYselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negYselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negYselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** +X measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + negYselfTestDataset.err = *(packet + offset); + offset += 2; // STEP byte will not be stored + negYselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negYselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negYselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negYselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negYselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negYselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negYselfTestDataset.coilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negYselfTestDataset.coilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negYselfTestDataset.coilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negYselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negYselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negYselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** FINA measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + negYselfTestDataset.finaErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + negYselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negYselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negYselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negYselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negYselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negYselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negYselfTestDataset.finaCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negYselfTestDataset.finaCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negYselfTestDataset.finaCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negYselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negYselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negYselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + +#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1 + sif::info << "IMTQ self test (INIT) err: " + << static_cast(negYselfTestDataset.initErr.value) << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field X: " << negYselfTestDataset.initRawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << negYselfTestDataset.initRawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << negYselfTestDataset.initRawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " + << negYselfTestDataset.initCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " + << negYselfTestDataset.initCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " + << negYselfTestDataset.initCalMagZ << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) coil X current: " << negYselfTestDataset.initCoilXCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y current: " << negYselfTestDataset.initCoilYCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z current: " << negYselfTestDataset.initCoilZCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil X temperature: " + << negYselfTestDataset.initCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y temperature: " + << negYselfTestDataset.initCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z temperature: " + << negYselfTestDataset.initCoilZTemperature << " °C" << std::endl; + + sif::info << "IMTQ self test (-Y) err: " + << static_cast(negYselfTestDataset.err.value) << std::endl; + sif::info << "IMTQ self test (-Y) raw magnetic field X: " << negYselfTestDataset.rawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (-Y) raw magnetic field Y: " << negYselfTestDataset.rawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (-Y) raw magnetic field Z: " << negYselfTestDataset.rawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (-Y) calibrated magnetic field X: " << negYselfTestDataset.calMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (-Y) calibrated magnetic field Y: " << negYselfTestDataset.calMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (-Y) calibrated magnetic field Z: " << negYselfTestDataset.calMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (-Y) coil X current: " << negYselfTestDataset.coilXCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (-Y) coil Y current: " << negYselfTestDataset.coilYCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (-Y) coil Z current: " << negYselfTestDataset.coilZCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (-Y) coil X temperature: " << negYselfTestDataset.coilXTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (-Y) coil Y temperature: " << negYselfTestDataset.coilYTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (-Y) coil Z temperature: " << negYselfTestDataset.coilZTemperature + << " °C" << std::endl; + + sif::info << "IMTQ self test (FINA) err: " + << static_cast(negYselfTestDataset.finaErr.value) << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field X: " << negYselfTestDataset.finaRawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << negYselfTestDataset.finaRawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << negYselfTestDataset.finaRawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " + << negYselfTestDataset.finaCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " + << negYselfTestDataset.finaCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " + << negYselfTestDataset.finaCalMagZ << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) coil X current: " << negYselfTestDataset.finaCoilXCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y current: " << negYselfTestDataset.finaCoilYCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z current: " << negYselfTestDataset.finaCoilZCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil X temperature: " + << negYselfTestDataset.finaCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y temperature: " + << negYselfTestDataset.finaCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z temperature: " + << negYselfTestDataset.finaCoilZTemperature << " °C" << std::endl; +#endif +} + +void IMTQHandler::handlePositiveZSelfTestReply(const uint8_t* packet) { + PoolReadGuard rg(&posXselfTestDataset); + + uint16_t offset = 2; + /** Init measurements */ + posZselfTestDataset.initErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + posZselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posZselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posZselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posZselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posZselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posZselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posZselfTestDataset.initCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posZselfTestDataset.initCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posZselfTestDataset.initCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posZselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posZselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posZselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** +X measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + posZselfTestDataset.err = *(packet + offset); + offset += 2; // STEP byte will not be stored + posZselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posZselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posZselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posZselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posZselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posZselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posZselfTestDataset.coilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posZselfTestDataset.coilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posZselfTestDataset.coilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posZselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posZselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posZselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** FINA measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + posZselfTestDataset.finaErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + posZselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posZselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posZselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + posZselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posZselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posZselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + posZselfTestDataset.finaCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posZselfTestDataset.finaCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posZselfTestDataset.finaCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + posZselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posZselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + posZselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + +#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1 + sif::info << "IMTQ self test (INIT) err: " + << static_cast(posZselfTestDataset.initErr.value) << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field X: " << posZselfTestDataset.initRawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << posZselfTestDataset.initRawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << posZselfTestDataset.initRawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " + << posZselfTestDataset.initCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " + << posZselfTestDataset.initCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " + << posZselfTestDataset.initCalMagZ << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) coil X current: " << posZselfTestDataset.initCoilXCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y current: " << posZselfTestDataset.initCoilYCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z current: " << posZselfTestDataset.initCoilZCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil X temperature: " + << posZselfTestDataset.initCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y temperature: " + << posZselfTestDataset.initCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z temperature: " + << posZselfTestDataset.initCoilZTemperature << " °C" << std::endl; + + sif::info << "IMTQ self test (+Z) err: " + << static_cast(posZselfTestDataset.err.value) << std::endl; + sif::info << "IMTQ self test (+Z) raw magnetic field X: " << posZselfTestDataset.rawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (+Z) raw magnetic field Y: " << posZselfTestDataset.rawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (+Z) raw magnetic field Z: " << posZselfTestDataset.rawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (+Z) calibrated magnetic field X: " << posZselfTestDataset.calMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (+Z) calibrated magnetic field Y: " << posZselfTestDataset.calMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (+Z) calibrated magnetic field Z: " << posZselfTestDataset.calMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (+Z) coil X current: " << posZselfTestDataset.coilXCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (+Z) coil Y current: " << posZselfTestDataset.coilYCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (+Z) coil Z current: " << posZselfTestDataset.coilZCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (+Z) coil X temperature: " << posZselfTestDataset.coilXTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (+Z) coil Y temperature: " << posZselfTestDataset.coilYTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (+Z) coil Z temperature: " << negYselfTestDataset.coilZTemperature + << " °C" << std::endl; + + sif::info << "IMTQ self test (FINA) err: " + << static_cast(posZselfTestDataset.finaErr.value) << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field X: " << posZselfTestDataset.finaRawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << posZselfTestDataset.finaRawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << posZselfTestDataset.finaRawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " + << posZselfTestDataset.finaCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " + << posZselfTestDataset.finaCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " + << posZselfTestDataset.finaCalMagZ << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) coil X current: " << posZselfTestDataset.finaCoilXCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y current: " << posZselfTestDataset.finaCoilYCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z current: " << posZselfTestDataset.finaCoilZCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil X temperature: " + << posZselfTestDataset.finaCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y temperature: " + << posZselfTestDataset.finaCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z temperature: " + << posZselfTestDataset.finaCoilZTemperature << " °C" << std::endl; +#endif +} + +void IMTQHandler::handleNegativeZSelfTestReply(const uint8_t* packet) { + PoolReadGuard rg(&posXselfTestDataset); + + uint16_t offset = 2; + /** Init measurements */ + negZselfTestDataset.initErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + negZselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negZselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negZselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negZselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negZselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negZselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negZselfTestDataset.initCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negZselfTestDataset.initCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negZselfTestDataset.initCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negZselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negZselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negZselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** +X measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + negZselfTestDataset.err = *(packet + offset); + offset += 2; // STEP byte will not be stored + negZselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negZselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negZselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negZselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negZselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negZselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negZselfTestDataset.coilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negZselfTestDataset.coilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negZselfTestDataset.coilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negZselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negZselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negZselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + + /** FINA measurements */ + checkErrorByte(*(packet + offset), *(packet + offset + 1)); + negZselfTestDataset.finaErr = *(packet + offset); + offset += 2; // STEP byte will not be stored + negZselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negZselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negZselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5; + offset += 4; + negZselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negZselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negZselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + negZselfTestDataset.finaCoilXCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negZselfTestDataset.finaCoilYCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negZselfTestDataset.finaCoilZCurrent = static_cast(*(packet + offset + 1) << 8 + | *(packet + offset)) * 0.1; + offset += 2; + negZselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negZselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 2; + negZselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset); + offset += 4; + +#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1 + sif::info << "IMTQ self test (INIT) err: " + << static_cast(negZselfTestDataset.initErr.value) << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field X: " << negZselfTestDataset.initRawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << negZselfTestDataset.initRawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << negZselfTestDataset.initRawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field X: " + << negZselfTestDataset.initCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: " + << negZselfTestDataset.initCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: " + << negZselfTestDataset.initCalMagZ << " nT" << std::endl; + sif::info << "IMTQ self test (INIT) coil X current: " << negZselfTestDataset.initCoilXCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y current: " << negZselfTestDataset.initCoilYCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z current: " << negZselfTestDataset.initCoilZCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (INIT) coil X temperature: " + << negZselfTestDataset.initCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Y temperature: " + << negZselfTestDataset.initCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (INIT) coil Z temperature: " + << negZselfTestDataset.initCoilZTemperature << " °C" << std::endl; + + sif::info << "IMTQ self test (-Z) err: " + << static_cast(negZselfTestDataset.err.value) << std::endl; + sif::info << "IMTQ self test (-Z) raw magnetic field X: " << negZselfTestDataset.rawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (-Z) raw magnetic field Y: " << negZselfTestDataset.rawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (-Z) raw magnetic field Z: " << negZselfTestDataset.rawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (-Z) calibrated magnetic field X: " << negZselfTestDataset.calMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (-Z) calibrated magnetic field Y: " << negZselfTestDataset.calMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (-Z) calibrated magnetic field Z: " << negZselfTestDataset.calMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (-Z) coil X current: " << negZselfTestDataset.coilXCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (-Z) coil Y current: " << negZselfTestDataset.coilYCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (-Z) coil Z current: " << negZselfTestDataset.coilZCurrent << " mA" + << std::endl; + sif::info << "IMTQ self test (-Z) coil X temperature: " << negZselfTestDataset.coilXTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (-Z) coil Y temperature: " << negZselfTestDataset.coilYTemperature + << " °C" << std::endl; + sif::info << "IMTQ self test (-Z) coil Z temperature: " << negYselfTestDataset.coilZTemperature + << " °C" << std::endl; + + sif::info << "IMTQ self test (FINA) err: " + << static_cast(negZselfTestDataset.finaErr.value) << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field X: " << negZselfTestDataset.finaRawMagX + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << negZselfTestDataset.finaRawMagY + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << negZselfTestDataset.finaRawMagZ + << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field X: " + << negZselfTestDataset.finaCalMagX << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: " + << negZselfTestDataset.finaCalMagY << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: " + << negZselfTestDataset.finaCalMagZ << " nT" << std::endl; + sif::info << "IMTQ self test (FINA) coil X current: " << negZselfTestDataset.finaCoilXCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y current: " << negZselfTestDataset.finaCoilYCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z current: " << negZselfTestDataset.finaCoilZCurrent + << " mA" << std::endl; + sif::info << "IMTQ self test (FINA) coil X temperature: " + << negZselfTestDataset.finaCoilXTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Y temperature: " + << negZselfTestDataset.finaCoilYTemperature << " °C" << std::endl; + sif::info << "IMTQ self test (FINA) coil Z temperature: " + << negZselfTestDataset.finaCoilZTemperature << " °C" << std::endl; +#endif +} + +void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) { + std::string stepString(""); + if (step < 8) { + stepString = makeStepString(step); + } else { + /** 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; +} + diff --git a/mission/devices/IMTQHandler.h b/mission/devices/IMTQHandler.h index e162c1c6..ce9e45e0 100644 --- a/mission/devices/IMTQHandler.h +++ b/mission/devices/IMTQHandler.h @@ -34,6 +34,7 @@ protected: ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; void setNormalDatapoolEntriesInvalid() override; + virtual LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override; uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) override; @@ -49,16 +50,48 @@ private: static const ReturnValue_t INTERNAL_PROCESSING_ERROR = MAKE_RETURN_CODE(0xA4); static const ReturnValue_t REJECTED_WITHOUT_REASON = MAKE_RETURN_CODE(0xA5); static const ReturnValue_t CMD_ERR_UNKNOWN = MAKE_RETURN_CODE(0xA6); + //! [EXPORT] : [COMMENT] The status reply to a self test command was received but no self test command has been sent. This should normally never happen. + static const ReturnValue_t UNEXPECTED_SELF_TEST_REPLY = MAKE_RETURN_CODE(0xA7); + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::IMTQ_HANDLER; + + //! [EXPORT] : [COMMENT] Get self test result returns I2C failure + //! P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA + static const Event SELF_TEST_I2C_FAILURE = MAKE_EVENT(1, severity::LOW); + //! [EXPORT] : [COMMENT] Get self test result returns SPI failure. This concerns the MTM connectivity. + //! P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA + static const Event SELF_TEST_SPI_FAILURE = MAKE_EVENT(2, severity::LOW); + //! [EXPORT] : [COMMENT] Get self test result returns failure in measurement of current and temperature. + //! P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA + static const Event SELF_TEST_ADC_FAILURE = MAKE_EVENT(3, severity::LOW); + //! [EXPORT] : [COMMENT] Get self test result returns PWM failure which concerns the coil actuation. + //! P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA + static const Event SELF_TEST_PWM_FAILURE = MAKE_EVENT(4, severity::LOW); + //! [EXPORT] : [COMMENT] Get self test result returns TC failure (system failure) + //! P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA + static const Event SELF_TEST_TC_FAILURE = MAKE_EVENT(5, severity::LOW); + //! [EXPORT] : [COMMENT] Get self test result returns failure that MTM values were outside of the expected range. + //! P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA + static const Event SELF_TEST_MTM_RANGE_FAILURE = MAKE_EVENT(6, severity::LOW); + //! [EXPORT] : [COMMENT] Get self test result returns failure indicating that the coil current was outside of the expected range + //! P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA + static const Event SELF_TEST_COIL_CURRENT_FAILURE = MAKE_EVENT(7, severity::LOW); + //! [EXPORT] : [COMMENT] Received invalid error byte. This indicates an error of the communication link between IMTQ and OBC. + static const Event INVALID_ERROR_BYTE = MAKE_EVENT(8, severity::LOW); IMTQ::EngHkDataset engHkDataset; IMTQ::CalibratedMtmMeasurementSet calMtmMeasurementSet; IMTQ::RawMtmMeasurementSet rawMtmMeasurementSet; + IMTQ::PosXSelfTestSet posXselfTestDataset; + IMTQ::NegXSelfTestSet negXselfTestDataset; + IMTQ::PosYSelfTestSet posYselfTestDataset; + IMTQ::NegYSelfTestSet negYselfTestDataset; + IMTQ::PosZSelfTestSet posZselfTestDataset; + IMTQ::NegZSelfTestSet negZselfTestDataset; uint8_t commandBuffer[IMTQ::MAX_COMMAND_SIZE]; enum class CommunicationStep { - SELF_TEST, GET_ENG_HK_DATA, START_MTM_MEASUREMENT, GET_CAL_MTM_MEASUREMENT, @@ -67,6 +100,22 @@ private: CommunicationStep communicationStep = CommunicationStep::GET_ENG_HK_DATA; + enum class StartupStep { + NONE, + COMMAND_SELF_TEST, + GET_SELF_TEST_RESULT + }; + + StartupStep startupStep = StartupStep::COMMAND_SELF_TEST; + + bool selfTestPerformed = false; + + /** + * @brief In case of a status reply to a single axis self test command, this function + * searches for the actual pending command. + */ + ReturnValue_t getSelfTestCommandId(DeviceCommandId_t* id); + /** * @brief Each reply contains a status byte giving information about a request. This function * parses this byte and returns the associated failure message. @@ -114,6 +163,33 @@ private: * command. */ void fillRawMtmDataset(const uint8_t* packet); + + /** + * @brief This function handles all self test results. This comprises parsing the error byte + * and step byte and calling the function to fill the respective dataset. + */ + void handleSelfTestReply(const uint8_t* packet); + + /** + * @brief The following functions fill the respective dataset of the single axis self tests. + * @param packet Pointer to the reply data holding the self test result. + */ + void handlePositiveXSelfTestReply(const uint8_t* packet); + void handleNegativeXSelfTestReply(const uint8_t* packet); + void handlePositiveYSelfTestReply(const uint8_t* packet); + void handleNegativeYSelfTestReply(const uint8_t* packet); + void handlePositiveZSelfTestReply(const uint8_t* packet); + void handleNegativeZSelfTestReply(const uint8_t* packet); + + /** + * @brief This function checks the error byte of a self test measurement. + * + * @param errorByte The received error byte to check + * @param step The step of the error byte. + */ + void checkErrorByte(const uint8_t errorByte, const uint8_t step); + + std::string makeStepString(const uint8_t step); }; #endif /* MISSION_DEVICES_IMTQHANDLER_H_ */ diff --git a/mission/devices/Max31865PT1000Handler.cpp b/mission/devices/Max31865PT1000Handler.cpp index c6d98c0a..12fd5102 100644 --- a/mission/devices/Max31865PT1000Handler.cpp +++ b/mission/devices/Max31865PT1000Handler.cpp @@ -8,7 +8,7 @@ Max31865PT1000Handler::Max31865PT1000Handler(object_id_t objectId, DeviceHandlerBase(objectId, comIF, comCookie), switchId(switchId), sensorDataset(this), sensorDatasetSid(sensorDataset.getSid()) { #if OBSW_VERBOSE_LEVEL >= 1 - debugDivider = new PeriodicOperationDivider(10); + debugDivider = new PeriodicOperationDivider(0); #endif } @@ -38,10 +38,38 @@ void Max31865PT1000Handler::doStartUp() { if(internalState == InternalState::REQUEST_CONFIG) { if (commandExecuted) { + commandExecuted = false; + internalState = InternalState::CONFIG_HIGH_THRESHOLD; + } + } + + if(internalState == InternalState::CONFIG_HIGH_THRESHOLD) { + if(commandExecuted) { + internalState = InternalState::REQUEST_HIGH_THRESHOLD; + commandExecuted = false; + } + } + + if(internalState == InternalState::REQUEST_HIGH_THRESHOLD) { + if(commandExecuted) { + internalState = InternalState::CONFIG_LOW_THRESHOLD; + commandExecuted = false; + } + } + + if(internalState == InternalState::CONFIG_LOW_THRESHOLD) { + if(commandExecuted) { + internalState = InternalState::REQUEST_LOW_THRESHOLD; + commandExecuted = false; + } + } + + if(internalState == InternalState::REQUEST_LOW_THRESHOLD) { + if(commandExecuted) { setMode(MODE_ON); setMode(MODE_NORMAL); - commandExecuted = false; internalState = InternalState::RUNNING; + commandExecuted = false; } } } @@ -82,6 +110,22 @@ ReturnValue_t Max31865PT1000Handler::buildTransitionDeviceCommand( *id = Max31865Definitions::REQUEST_CONFIG; return buildCommandFromCommand(*id, nullptr, 0); } + case(InternalState::CONFIG_HIGH_THRESHOLD): { + *id = Max31865Definitions::WRITE_HIGH_THRESHOLD; + return buildCommandFromCommand(*id, nullptr, 0); + } + case(InternalState::REQUEST_HIGH_THRESHOLD): { + *id = Max31865Definitions::REQUEST_HIGH_THRESHOLD; + return buildCommandFromCommand(*id, nullptr, 0); + } + case(InternalState::CONFIG_LOW_THRESHOLD): { + *id = Max31865Definitions::WRITE_LOW_THRESHOLD; + return buildCommandFromCommand(*id, nullptr, 0); + } + case(InternalState::REQUEST_LOW_THRESHOLD): { + *id = Max31865Definitions::REQUEST_LOW_THRESHOLD; + return buildCommandFromCommand(*id, nullptr, 0); + } default: #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -110,13 +154,49 @@ ReturnValue_t Max31865PT1000Handler::buildCommandFromCommand( } } case(Max31865Definitions::REQUEST_CONFIG): { - commandBuffer[0] = 0x00; // dummy byte - commandBuffer[1] = static_cast( + commandBuffer[0] = static_cast( Max31865Definitions::REQUEST_CONFIG); + commandBuffer[1] = 0x00; // dummy byte DeviceHandlerBase::rawPacketLen = 2; DeviceHandlerBase::rawPacket = commandBuffer.data(); return HasReturnvaluesIF::RETURN_OK; } + case(Max31865Definitions::WRITE_HIGH_THRESHOLD): { + commandBuffer[0] = static_cast( + Max31865Definitions::WRITE_HIGH_THRESHOLD); + commandBuffer[1] = static_cast(HIGH_THRESHOLD >> 8); + commandBuffer[2] = static_cast(HIGH_THRESHOLD & 0xFF); + DeviceHandlerBase::rawPacketLen = 3; + DeviceHandlerBase::rawPacket = commandBuffer.data(); + return HasReturnvaluesIF::RETURN_OK; + } + case(Max31865Definitions::REQUEST_HIGH_THRESHOLD): { + commandBuffer[0] = static_cast( + Max31865Definitions::REQUEST_HIGH_THRESHOLD); + commandBuffer[1] = 0x00; //dummy byte + commandBuffer[2] = 0x00; //dummy byte + DeviceHandlerBase::rawPacketLen = 3; + DeviceHandlerBase::rawPacket = commandBuffer.data(); + return HasReturnvaluesIF::RETURN_OK; + } + case(Max31865Definitions::WRITE_LOW_THRESHOLD): { + commandBuffer[0] = static_cast( + Max31865Definitions::WRITE_LOW_THRESHOLD); + commandBuffer[1] = static_cast(LOW_THRESHOLD >> 8); + commandBuffer[2] = static_cast(LOW_THRESHOLD & 0xFF); + DeviceHandlerBase::rawPacketLen = 3; + DeviceHandlerBase::rawPacket = commandBuffer.data(); + return HasReturnvaluesIF::RETURN_OK; + } + case(Max31865Definitions::REQUEST_LOW_THRESHOLD): { + commandBuffer[0] = static_cast( + Max31865Definitions::REQUEST_LOW_THRESHOLD); + commandBuffer[1] = 0x00; //dummy byte + commandBuffer[2] = 0x00; //dummy byte + DeviceHandlerBase::rawPacketLen = 3; + DeviceHandlerBase::rawPacket = commandBuffer.data(); + return HasReturnvaluesIF::RETURN_OK; + } case(Max31865Definitions::REQUEST_RTD): { commandBuffer[0] = static_cast( Max31865Definitions::REQUEST_RTD); @@ -144,6 +224,10 @@ ReturnValue_t Max31865PT1000Handler::buildCommandFromCommand( void Max31865PT1000Handler::fillCommandAndReplyMap() { insertInCommandAndReplyMap(Max31865Definitions::CONFIG_CMD, 3); insertInCommandAndReplyMap(Max31865Definitions::REQUEST_CONFIG, 3); + insertInCommandAndReplyMap(Max31865Definitions::WRITE_LOW_THRESHOLD, 3); + insertInCommandAndReplyMap(Max31865Definitions::REQUEST_LOW_THRESHOLD, 3); + insertInCommandAndReplyMap(Max31865Definitions::WRITE_HIGH_THRESHOLD, 3); + insertInCommandAndReplyMap(Max31865Definitions::REQUEST_HIGH_THRESHOLD, 3); insertInCommandAndReplyMap(Max31865Definitions::REQUEST_RTD, 3, &sensorDataset); insertInCommandAndReplyMap(Max31865Definitions::REQUEST_FAULT_BYTE, 3); @@ -158,6 +242,39 @@ ReturnValue_t Max31865PT1000Handler::scanForReply(const uint8_t *start, internalState == InternalState::RUNNING) { *foundId = Max31865Definitions::REQUEST_RTD; *foundLen = rtdReplySize; + return RETURN_OK; + } + + if(remainingSize == 3) { + switch(internalState) { + case(InternalState::CONFIG_HIGH_THRESHOLD): { + *foundLen = 3; + *foundId = Max31865Definitions::WRITE_HIGH_THRESHOLD; + commandExecuted = true; + return RETURN_OK; + } + case(InternalState::REQUEST_HIGH_THRESHOLD): { + *foundLen = 3; + *foundId = Max31865Definitions::REQUEST_HIGH_THRESHOLD; + return RETURN_OK; + } + case(InternalState::CONFIG_LOW_THRESHOLD): { + *foundLen = 3; + *foundId = Max31865Definitions::WRITE_LOW_THRESHOLD; + commandExecuted = true; + return RETURN_OK; + } + case(InternalState::REQUEST_LOW_THRESHOLD): { + *foundLen = 3; + *foundId = Max31865Definitions::REQUEST_LOW_THRESHOLD; + return RETURN_OK; + } + default: { + sif::debug << "Max31865PT1000Handler::scanForReply: Unknown internal state" + << std::endl; + return RETURN_OK; + } + } } if(remainingSize == configReplySize) { @@ -203,14 +320,38 @@ ReturnValue_t Max31865PT1000Handler::interpretDeviceReply( } break; } + case(Max31865Definitions::REQUEST_LOW_THRESHOLD): { + uint16_t readLowThreshold = packet[0] << 8 | packet[1]; + if(readLowThreshold != LOW_THRESHOLD) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 && DEBUG_RTD == 1 + sif::error + << "Max31865PT1000Handler::interpretDeviceReply: Missmatch between written " + << "and readback value of low threshold register" + << std::endl; +#endif + } + commandExecuted = true; + break; + } + case(Max31865Definitions::REQUEST_HIGH_THRESHOLD): { + uint16_t readHighThreshold = packet[0] << 8 | packet[1]; + if(readHighThreshold != HIGH_THRESHOLD) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 && DEBUG_RTD == 1 + sif::error + << "Max31865PT1000Handler::interpretDeviceReply: Missmatch between written " + << "and readback value of high threshold register" + << std::endl; +#endif + } + commandExecuted = true; + break; + } case(Max31865Definitions::REQUEST_RTD): { // first bit of LSB reply byte is the fault bit uint8_t faultBit = packet[2] & 0b0000'0001; if(faultBit == 1) { // Maybe we should attempt to restart it? - if(faultByte == 0) { - internalState = InternalState::REQUEST_FAULT_BYTE; - } + internalState = InternalState::REQUEST_FAULT_BYTE; } // RTD value consists of last seven bits of the LSB reply byte and @@ -329,7 +470,7 @@ void Max31865PT1000Handler::debugInterface(uint8_t positionTracker, uint32_t Max31865PT1000Handler::getTransitionDelayMs( Mode_t modeFrom, Mode_t modeTo) { - return 5000; + return 20000; } ReturnValue_t Max31865PT1000Handler::getSwitches( @@ -348,8 +489,8 @@ ReturnValue_t Max31865PT1000Handler::initializeLocalDataPool(localpool::DataPool new PoolEntry({0}, 1, true)); localDataPoolMap.emplace(Max31865Definitions::PoolIds::FAULT_BYTE, new PoolEntry({0})); - poolManager.subscribeForPeriodicPacket(sensorDatasetSid, - false, 4.0, false); +// poolManager.subscribeForPeriodicPacket(sensorDatasetSid, +// false, 4.0, false); return HasReturnvaluesIF::RETURN_OK; } diff --git a/mission/devices/Max31865PT1000Handler.h b/mission/devices/Max31865PT1000Handler.h index 19780df2..f3240788 100644 --- a/mission/devices/Max31865PT1000Handler.h +++ b/mission/devices/Max31865PT1000Handler.h @@ -37,16 +37,27 @@ public: // Configuration in 8 digit code: // 1. 1 for V_BIAS enabled, 0 for disabled // 2. 1 for Auto-conversion, 0 for off - // 3. 1 for 1-shot enabled, 0 for disabled + // 3. 1 for 1-shot enabled, 0 for disabled (self-clearing bit) // 4. 1 for 3-wire disabled, 0 for disabled // 5./6. Fault detection: 00 for no action, 01 for automatic delay, 1 // 0 for run fault detection with manual delay, // 11 for finish fault detection with manual delay - // 7. Fault status: 1 for auto-clear, 0 for auto-clear off + // 7. Fault status: Write 1 to reset fault status register (Bit is self cleared afterwards) // 8. 1 for 50 Hz filter, 0 for 60 Hz filter (noise rejection filter) static constexpr uint8_t DEFAULT_CONFIG = 0b11000001; - static constexpr float RTD_RREF_PT1000 = 4000.0; //!< Ohm + /** + * Expected temperature range is -100°C and 100°C. + * If there are temperatures beyond this range there must be a fault. + * The threshold variables cause the MAX1385 to signal an error in case the measured resistance + * indicates a temperature higher than 100°C or lower than -100°C. + * Default settings of MAX13865 are 0xFFFF for the high threshold register and 0x0 for the + * low threshold register. + */ + static constexpr uint16_t HIGH_THRESHOLD = 11298; // = 100°C + static constexpr uint16_t LOW_THRESHOLD = 4902; // = -100°C + + static constexpr float RTD_RREF_PT1000 = 4020.0; //!< Ohm static constexpr float RTD_RESISTANCE0_PT1000 = 1000.0; //!< Ohm protected: /* DeviceHandlerBase abstract function implementation */ @@ -80,6 +91,10 @@ private: NONE, WARMUP, CONFIGURE, + CONFIG_HIGH_THRESHOLD, + REQUEST_HIGH_THRESHOLD, + CONFIG_LOW_THRESHOLD, + REQUEST_LOW_THRESHOLD, REQUEST_CONFIG, RUNNING, REQUEST_FAULT_BYTE diff --git a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h index 76dd85c5..e95bd42e 100644 --- a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h +++ b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h @@ -1,6 +1,8 @@ #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_ #define MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_ +#include + namespace IMTQ { static const DeviceCommandId_t NONE = 0x0; @@ -13,14 +15,27 @@ namespace IMTQ { static const DeviceCommandId_t GET_CAL_MTM_MEASUREMENT = 0x5; /** Requests the raw values measured by the built-in MTM XEN1210 */ static const DeviceCommandId_t GET_RAW_MTM_MEASUREMENT = 0x6; - static const DeviceCommandId_t SELF_TEST = 0x7; + static const DeviceCommandId_t POS_X_SELF_TEST = 0x7; + static const DeviceCommandId_t NEG_X_SELF_TEST = 0x8; + static const DeviceCommandId_t POS_Y_SELF_TEST = 0x9; + static const DeviceCommandId_t NEG_Y_SELF_TEST = 0xA; + static const DeviceCommandId_t POS_Z_SELF_TEST = 0xB; + static const DeviceCommandId_t NEG_Z_SELF_TEST = 0xC; + static const DeviceCommandId_t GET_SELF_TEST_RESULT = 0xD; static const uint8_t GET_TEMP_REPLY_SIZE = 2; static const uint8_t CFGR_CMD_SIZE = 3; static const uint8_t POINTER_REG_SIZE = 1; - static const uint32_t ENG_HK_DATA_SET_ID = GET_ENG_HK_DATA; - static const uint32_t CAL_MTM_SET = GET_CAL_MTM_MEASUREMENT; + static const uint32_t ENG_HK_DATA_SET_ID = 1; + static const uint32_t CAL_MTM_SET = 2; + static const uint32_t RAW_MTM_SET = 3; + static const uint32_t POS_X_TEST_DATASET = 4; + static const uint32_t NEG_X_TEST_DATASET = 5; + static const uint32_t POS_Y_TEST_DATASET = 6; + static const uint32_t NEG_Y_TEST_DATASET = 7; + static const uint32_t POS_Z_TEST_DATASET = 8; + static const uint32_t NEG_Z_TEST_DATASET = 9; static const uint8_t SIZE_ENG_HK_COMMAND = 1; static const uint8_t SIZE_STATUS_REPLY = 2; @@ -28,13 +43,27 @@ namespace IMTQ { static const uint8_t SIZE_GET_COMMANDED_DIPOLE_REPLY = 8; static const uint8_t SIZE_GET_CAL_MTM_MEASUREMENT = 15; static const uint8_t SIZE_GET_RAW_MTM_MEASUREMENT = 15; + static const uint16_t SIZE_SELF_TEST_RESULTS = 120; - static const uint8_t MAX_REPLY_SIZE = SIZE_ENG_HK_DATA_REPLY; + static const uint16_t MAX_REPLY_SIZE = SIZE_SELF_TEST_RESULTS; static const uint8_t MAX_COMMAND_SIZE = 9; /** Define entries in IMTQ specific dataset */ static const uint8_t ENG_HK_SET_POOL_ENTRIES = 11; static const uint8_t CAL_MTM_POOL_ENTRIES = 4; + static const uint8_t SELF_TEST_DATASET_ENTRIES = 104; + + /** Error codes for interpreting the self test error byte */ + static const uint8_t I2C_FAILURE_MASK = 0x1; + static const uint8_t SPI_FAILURE_MASK = 0x2; // MTM connectivity + static const uint8_t ADC_FAILURE_MASK = 0x4; // Current/Temp measurement + static const uint8_t PWM_FAILURE_MASK = 0x8; // Coil actuation + static const uint8_t TC_FAILURE_MASK = 0x10; // System failure + static const uint8_t MTM_RANGE_FAILURE_MASK = 0x20; // MTM values outside of expected range + static const uint8_t COIL_CURRENT_FAILURE_MASK = 0x40; // Coil currents outside of expected range + static const uint8_t INVALID_ERROR_BYTE = 0x80; // This is an invalid error byte and should be never replied by the IMTQ + + static const uint8_t MAIN_STEP_OFFSET = 43; /** * Command code definitions. Each command or reply of an IMTQ request will begin with one of @@ -43,13 +72,36 @@ namespace IMTQ { namespace CC { static const uint8_t START_MTM_MEASUREMENT = 0x4; static const uint8_t START_ACTUATION_DIPOLE = 0x6; + static const uint8_t SELF_TEST_CMD = 0x8; static const uint8_t SOFTWARE_RESET = 0xAA; static const uint8_t GET_ENG_HK_DATA = 0x4A; static const uint8_t GET_COMMANDED_DIPOLE = 0x46; static const uint8_t GET_RAW_MTM_MEASUREMENT = 0x42; static const uint8_t GET_CAL_MTM_MEASUREMENT = 0x43; + static const uint8_t GET_SELF_TEST_RESULT = 0x47; }; + namespace SELF_TEST_AXIS { + static const uint8_t ALL = 0x0; + static const uint8_t X_POSITIVE = 0x1; + static const uint8_t X_NEGATIVE = 0x2; + static const uint8_t Y_POSITIVE = 0x3; + static const uint8_t Y_NEGATIVE = 0x4; + static const uint8_t Z_POSITIVE = 0x5; + static const uint8_t Z_NEGATIVE = 0x6; + } + + namespace SELF_TEST_STEPS { + static const uint8_t INIT = 0x0; + static const uint8_t X_POSITIVE = 0x1; + static const uint8_t X_NEGATIVE = 0x2; + static const uint8_t Y_POSITIVE = 0x3; + static const uint8_t Y_NEGATIVE = 0x4; + static const uint8_t Z_POSITIVE = 0x5; + static const uint8_t Z_NEGATIVE = 0x6; + static const uint8_t FINA = 0x7; + } + enum IMTQPoolIds: lp_id_t { DIGITAL_VOLTAGE_MV, ANALOG_VOLTAGE_MV, @@ -69,100 +121,337 @@ namespace IMTQ { MTM_RAW_X, MTM_RAW_Y, MTM_RAW_Z, - ACTUATION_RAW_STATUS + ACTUATION_RAW_STATUS, + + INIT_POS_X_ERR, + INIT_POS_X_RAW_MAG_X, + INIT_POS_X_RAW_MAG_Y, + INIT_POS_X_RAW_MAG_Z, + INIT_POS_X_CAL_MAG_X, + INIT_POS_X_CAL_MAG_Y, + INIT_POS_X_CAL_MAG_Z, + INIT_POS_X_COIL_X_CURRENT, + INIT_POS_X_COIL_Y_CURRENT, + INIT_POS_X_COIL_Z_CURRENT, + INIT_POS_X_COIL_X_TEMPERATURE, + INIT_POS_X_COIL_Y_TEMPERATURE, + INIT_POS_X_COIL_Z_TEMPERATURE, + + INIT_NEG_X_ERR, + INIT_NEG_X_RAW_MAG_X, + INIT_NEG_X_RAW_MAG_Y, + INIT_NEG_X_RAW_MAG_Z, + INIT_NEG_X_CAL_MAG_X, + INIT_NEG_X_CAL_MAG_Y, + INIT_NEG_X_CAL_MAG_Z, + INIT_NEG_X_COIL_X_CURRENT, + INIT_NEG_X_COIL_Y_CURRENT, + INIT_NEG_X_COIL_Z_CURRENT, + INIT_NEG_X_COIL_X_TEMPERATURE, + INIT_NEG_X_COIL_Y_TEMPERATURE, + INIT_NEG_X_COIL_Z_TEMPERATURE, + + INIT_POS_Y_ERR, + INIT_POS_Y_RAW_MAG_X, + INIT_POS_Y_RAW_MAG_Y, + INIT_POS_Y_RAW_MAG_Z, + INIT_POS_Y_CAL_MAG_X, + INIT_POS_Y_CAL_MAG_Y, + INIT_POS_Y_CAL_MAG_Z, + INIT_POS_Y_COIL_X_CURRENT, + INIT_POS_Y_COIL_Y_CURRENT, + INIT_POS_Y_COIL_Z_CURRENT, + INIT_POS_Y_COIL_X_TEMPERATURE, + INIT_POS_Y_COIL_Y_TEMPERATURE, + INIT_POS_Y_COIL_Z_TEMPERATURE, + + INIT_NEG_Y_ERR, + INIT_NEG_Y_RAW_MAG_X, + INIT_NEG_Y_RAW_MAG_Y, + INIT_NEG_Y_RAW_MAG_Z, + INIT_NEG_Y_CAL_MAG_X, + INIT_NEG_Y_CAL_MAG_Y, + INIT_NEG_Y_CAL_MAG_Z, + INIT_NEG_Y_COIL_X_CURRENT, + INIT_NEG_Y_COIL_Y_CURRENT, + INIT_NEG_Y_COIL_Z_CURRENT, + INIT_NEG_Y_COIL_X_TEMPERATURE, + INIT_NEG_Y_COIL_Y_TEMPERATURE, + INIT_NEG_Y_COIL_Z_TEMPERATURE, + + INIT_POS_Z_ERR, + INIT_POS_Z_RAW_MAG_X, + INIT_POS_Z_RAW_MAG_Y, + INIT_POS_Z_RAW_MAG_Z, + INIT_POS_Z_CAL_MAG_X, + INIT_POS_Z_CAL_MAG_Y, + INIT_POS_Z_CAL_MAG_Z, + INIT_POS_Z_COIL_X_CURRENT, + INIT_POS_Z_COIL_Y_CURRENT, + INIT_POS_Z_COIL_Z_CURRENT, + INIT_POS_Z_COIL_X_TEMPERATURE, + INIT_POS_Z_COIL_Y_TEMPERATURE, + INIT_POS_Z_COIL_Z_TEMPERATURE, + + INIT_NEG_Z_ERR, + INIT_NEG_Z_RAW_MAG_X, + INIT_NEG_Z_RAW_MAG_Y, + INIT_NEG_Z_RAW_MAG_Z, + INIT_NEG_Z_CAL_MAG_X, + INIT_NEG_Z_CAL_MAG_Y, + INIT_NEG_Z_CAL_MAG_Z, + INIT_NEG_Z_COIL_X_CURRENT, + INIT_NEG_Z_COIL_Y_CURRENT, + INIT_NEG_Z_COIL_Z_CURRENT, + INIT_NEG_Z_COIL_X_TEMPERATURE, + INIT_NEG_Z_COIL_Y_TEMPERATURE, + INIT_NEG_Z_COIL_Z_TEMPERATURE, + + POS_X_ERR, + POS_X_RAW_MAG_X, + POS_X_RAW_MAG_Y, + POS_X_RAW_MAG_Z, + POS_X_CAL_MAG_X, + POS_X_CAL_MAG_Y, + POS_X_CAL_MAG_Z, + POS_X_COIL_X_CURRENT, + POS_X_COIL_Y_CURRENT, + POS_X_COIL_Z_CURRENT, + POS_X_COIL_X_TEMPERATURE, + POS_X_COIL_Y_TEMPERATURE, + POS_X_COIL_Z_TEMPERATURE, + + NEG_X_ERR, + NEG_X_RAW_MAG_X, + NEG_X_RAW_MAG_Y, + NEG_X_RAW_MAG_Z, + NEG_X_CAL_MAG_X, + NEG_X_CAL_MAG_Y, + NEG_X_CAL_MAG_Z, + NEG_X_COIL_X_CURRENT, + NEG_X_COIL_Y_CURRENT, + NEG_X_COIL_Z_CURRENT, + NEG_X_COIL_X_TEMPERATURE, + NEG_X_COIL_Y_TEMPERATURE, + NEG_X_COIL_Z_TEMPERATURE, + + POS_Y_ERR, + POS_Y_RAW_MAG_X, + POS_Y_RAW_MAG_Y, + POS_Y_RAW_MAG_Z, + POS_Y_CAL_MAG_X, + POS_Y_CAL_MAG_Y, + POS_Y_CAL_MAG_Z, + POS_Y_COIL_X_CURRENT, + POS_Y_COIL_Y_CURRENT, + POS_Y_COIL_Z_CURRENT, + POS_Y_COIL_X_TEMPERATURE, + POS_Y_COIL_Y_TEMPERATURE, + POS_Y_COIL_Z_TEMPERATURE, + + NEG_Y_ERR, + NEG_Y_RAW_MAG_X, + NEG_Y_RAW_MAG_Y, + NEG_Y_RAW_MAG_Z, + NEG_Y_CAL_MAG_X, + NEG_Y_CAL_MAG_Y, + NEG_Y_CAL_MAG_Z, + NEG_Y_COIL_X_CURRENT, + NEG_Y_COIL_Y_CURRENT, + NEG_Y_COIL_Z_CURRENT, + NEG_Y_COIL_X_TEMPERATURE, + NEG_Y_COIL_Y_TEMPERATURE, + NEG_Y_COIL_Z_TEMPERATURE, + + POS_Z_ERR, + POS_Z_RAW_MAG_X, + POS_Z_RAW_MAG_Y, + POS_Z_RAW_MAG_Z, + POS_Z_CAL_MAG_X, + POS_Z_CAL_MAG_Y, + POS_Z_CAL_MAG_Z, + POS_Z_COIL_X_CURRENT, + POS_Z_COIL_Y_CURRENT, + POS_Z_COIL_Z_CURRENT, + POS_Z_COIL_X_TEMPERATURE, + POS_Z_COIL_Y_TEMPERATURE, + POS_Z_COIL_Z_TEMPERATURE, + + NEG_Z_ERR, + NEG_Z_RAW_MAG_X, + NEG_Z_RAW_MAG_Y, + NEG_Z_RAW_MAG_Z, + NEG_Z_CAL_MAG_X, + NEG_Z_CAL_MAG_Y, + NEG_Z_CAL_MAG_Z, + NEG_Z_COIL_X_CURRENT, + NEG_Z_COIL_Y_CURRENT, + NEG_Z_COIL_Z_CURRENT, + NEG_Z_COIL_X_TEMPERATURE, + NEG_Z_COIL_Y_TEMPERATURE, + NEG_Z_COIL_Z_TEMPERATURE, + + FINA_POS_X_ERR, + FINA_POS_X_RAW_MAG_X, + FINA_POS_X_RAW_MAG_Y, + FINA_POS_X_RAW_MAG_Z, + FINA_POS_X_CAL_MAG_X, + FINA_POS_X_CAL_MAG_Y, + FINA_POS_X_CAL_MAG_Z, + FINA_POS_X_COIL_X_CURRENT, + FINA_POS_X_COIL_Y_CURRENT, + FINA_POS_X_COIL_Z_CURRENT, + FINA_POS_X_COIL_X_TEMPERATURE, + FINA_POS_X_COIL_Y_TEMPERATURE, + FINA_POS_X_COIL_Z_TEMPERATURE, + + FINA_NEG_X_ERR, + FINA_NEG_X_RAW_MAG_X, + FINA_NEG_X_RAW_MAG_Y, + FINA_NEG_X_RAW_MAG_Z, + FINA_NEG_X_CAL_MAG_X, + FINA_NEG_X_CAL_MAG_Y, + FINA_NEG_X_CAL_MAG_Z, + FINA_NEG_X_COIL_X_CURRENT, + FINA_NEG_X_COIL_Y_CURRENT, + FINA_NEG_X_COIL_Z_CURRENT, + FINA_NEG_X_COIL_X_TEMPERATURE, + FINA_NEG_X_COIL_Y_TEMPERATURE, + FINA_NEG_X_COIL_Z_TEMPERATURE, + + FINA_POS_Y_ERR, + FINA_POS_Y_RAW_MAG_X, + FINA_POS_Y_RAW_MAG_Y, + FINA_POS_Y_RAW_MAG_Z, + FINA_POS_Y_CAL_MAG_X, + FINA_POS_Y_CAL_MAG_Y, + FINA_POS_Y_CAL_MAG_Z, + FINA_POS_Y_COIL_X_CURRENT, + FINA_POS_Y_COIL_Y_CURRENT, + FINA_POS_Y_COIL_Z_CURRENT, + FINA_POS_Y_COIL_X_TEMPERATURE, + FINA_POS_Y_COIL_Y_TEMPERATURE, + FINA_POS_Y_COIL_Z_TEMPERATURE, + + FINA_NEG_Y_ERR, + FINA_NEG_Y_RAW_MAG_X, + FINA_NEG_Y_RAW_MAG_Y, + FINA_NEG_Y_RAW_MAG_Z, + FINA_NEG_Y_CAL_MAG_X, + FINA_NEG_Y_CAL_MAG_Y, + FINA_NEG_Y_CAL_MAG_Z, + FINA_NEG_Y_COIL_X_CURRENT, + FINA_NEG_Y_COIL_Y_CURRENT, + FINA_NEG_Y_COIL_Z_CURRENT, + FINA_NEG_Y_COIL_X_TEMPERATURE, + FINA_NEG_Y_COIL_Y_TEMPERATURE, + FINA_NEG_Y_COIL_Z_TEMPERATURE, + + FINA_POS_Z_ERR, + FINA_POS_Z_RAW_MAG_X, + FINA_POS_Z_RAW_MAG_Y, + FINA_POS_Z_RAW_MAG_Z, + FINA_POS_Z_CAL_MAG_X, + FINA_POS_Z_CAL_MAG_Y, + FINA_POS_Z_CAL_MAG_Z, + FINA_POS_Z_COIL_X_CURRENT, + FINA_POS_Z_COIL_Y_CURRENT, + FINA_POS_Z_COIL_Z_CURRENT, + FINA_POS_Z_COIL_X_TEMPERATURE, + FINA_POS_Z_COIL_Y_TEMPERATURE, + FINA_POS_Z_COIL_Z_TEMPERATURE, + + FINA_NEG_Z_ERR, + FINA_NEG_Z_RAW_MAG_X, + FINA_NEG_Z_RAW_MAG_Y, + FINA_NEG_Z_RAW_MAG_Z, + FINA_NEG_Z_CAL_MAG_X, + FINA_NEG_Z_CAL_MAG_Y, + FINA_NEG_Z_CAL_MAG_Z, + FINA_NEG_Z_COIL_X_CURRENT, + FINA_NEG_Z_COIL_Y_CURRENT, + FINA_NEG_Z_COIL_Z_CURRENT, + FINA_NEG_Z_COIL_X_TEMPERATURE, + FINA_NEG_Z_COIL_Y_TEMPERATURE, + FINA_NEG_Z_COIL_Z_TEMPERATURE, }; -class EngHkDataset: - public StaticLocalDataSet { +class EngHkDataset: public StaticLocalDataSet { public: - EngHkDataset(HasLocalDataPoolIF* owner): - StaticLocalDataSet(owner, ENG_HK_DATA_SET_ID) { - } - - EngHkDataset(object_id_t objectId): - StaticLocalDataSet(sid_t(objectId, ENG_HK_DATA_SET_ID)) { - } - - lp_var_t digitalVoltageMv = lp_var_t(sid.objectId, - DIGITAL_VOLTAGE_MV, this); - lp_var_t analogVoltageMv = lp_var_t(sid.objectId, - ANALOG_VOLTAGE_MV, this); - lp_var_t digitalCurrentmA = lp_var_t(sid.objectId, - DIGITAL_CURRENT, this); - lp_var_t analogCurrentmA = lp_var_t(sid.objectId, - ANALOG_CURRENT, this); - lp_var_t coilXCurrentmA = lp_var_t(sid.objectId, - COIL_X_CURRENT, this); - lp_var_t coilYCurrentmA = lp_var_t(sid.objectId, - COIL_Y_CURRENT, this); - lp_var_t coilZCurrentmA = lp_var_t(sid.objectId, - COIL_Z_CURRENT, this); - /** All temperatures in [�C] */ - lp_var_t coilXTemperature = lp_var_t(sid.objectId, - COIL_X_TEMPERATURE, this); - lp_var_t coilYTemperature = lp_var_t(sid.objectId, - COIL_Y_TEMPERATURE, this); - lp_var_t coilZTemperature = lp_var_t(sid.objectId, - COIL_Z_TEMPERATURE, this); - lp_var_t mcuTemperature = lp_var_t(sid.objectId, - MCU_TEMPERATURE, this); -}; - -/** - * @brief This dataset holds the raw MTM measurements. - */ -class CalibratedMtmMeasurementSet: - public StaticLocalDataSet { -public: - - CalibratedMtmMeasurementSet(HasLocalDataPoolIF* owner): - StaticLocalDataSet(owner, CAL_MTM_SET) { + EngHkDataset(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, ENG_HK_DATA_SET_ID) { } - CalibratedMtmMeasurementSet(object_id_t objectId): - StaticLocalDataSet(sid_t(objectId, CAL_MTM_SET)) { + EngHkDataset(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, ENG_HK_DATA_SET_ID)) { } - /** The unit of all measurements is nT */ - lp_var_t mtmXnT = lp_var_t(sid.objectId, - MTM_CAL_X, this); - lp_var_t mtmYnT = lp_var_t(sid.objectId, - MTM_CAL_Y, this); - lp_var_t mtmZnT = lp_var_t(sid.objectId, - MTM_CAL_Z, this); - /** 1 if coils were actuating during measurement otherwise 0 */ - lp_var_t coilActuationStatus = lp_var_t(sid.objectId, - ACTUATION_CAL_STATUS, this); + lp_var_t digitalVoltageMv = lp_var_t(sid.objectId, DIGITAL_VOLTAGE_MV, + this); + lp_var_t analogVoltageMv = lp_var_t(sid.objectId, ANALOG_VOLTAGE_MV, this); + lp_var_t digitalCurrentmA = lp_var_t(sid.objectId, DIGITAL_CURRENT, this); + lp_var_t analogCurrentmA = lp_var_t(sid.objectId, ANALOG_CURRENT, this); + lp_var_t coilXCurrentmA = lp_var_t(sid.objectId, COIL_X_CURRENT, this); + lp_var_t coilYCurrentmA = lp_var_t(sid.objectId, COIL_Y_CURRENT, this); + lp_var_t coilZCurrentmA = lp_var_t(sid.objectId, COIL_Z_CURRENT, this); + /** All temperatures in [°C] */ + lp_var_t coilXTemperature = lp_var_t(sid.objectId, COIL_X_TEMPERATURE, + this); + lp_var_t coilYTemperature = lp_var_t(sid.objectId, COIL_Y_TEMPERATURE, + this); + lp_var_t coilZTemperature = lp_var_t(sid.objectId, COIL_Z_TEMPERATURE, + this); + lp_var_t mcuTemperature = lp_var_t(sid.objectId, MCU_TEMPERATURE, this); }; /** * @brief This dataset holds the last calibrated MTM measurement. */ -class RawMtmMeasurementSet: - public StaticLocalDataSet { +class CalibratedMtmMeasurementSet: public StaticLocalDataSet { public: - RawMtmMeasurementSet(HasLocalDataPoolIF* owner): - StaticLocalDataSet(owner, CAL_MTM_SET) { + CalibratedMtmMeasurementSet(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, CAL_MTM_SET) { } - RawMtmMeasurementSet(object_id_t objectId): - StaticLocalDataSet(sid_t(objectId, CAL_MTM_SET)) { + CalibratedMtmMeasurementSet(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, CAL_MTM_SET)) { } /** The unit of all measurements is nT */ - lp_var_t mtmXnT = lp_var_t(sid.objectId, - MTM_RAW_X, this); - lp_var_t mtmYnT = lp_var_t(sid.objectId, - MTM_RAW_Y, this); - lp_var_t mtmZnT = lp_var_t(sid.objectId, - MTM_RAW_Z, this); + lp_var_t mtmXnT = lp_var_t(sid.objectId, MTM_CAL_X, this); + lp_var_t mtmYnT = lp_var_t(sid.objectId, MTM_CAL_Y, this); + lp_var_t mtmZnT = lp_var_t(sid.objectId, MTM_CAL_Z, this); /** 1 if coils were actuating during measurement otherwise 0 */ - lp_var_t coilActuationStatus = lp_var_t(sid.objectId, - ACTUATION_RAW_STATUS, this); + lp_var_t coilActuationStatus = lp_var_t(sid.objectId, ACTUATION_CAL_STATUS, + this); }; +/** + * @brief This dataset holds the raw MTM measurements. + */ +class RawMtmMeasurementSet: public StaticLocalDataSet { +public: + + RawMtmMeasurementSet(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, RAW_MTM_SET) { + } + + RawMtmMeasurementSet(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, RAW_MTM_SET)) { + } + + /** The unit of all measurements is nT */ + lp_var_t mtmXnT = lp_var_t(sid.objectId, MTM_RAW_X, this); + lp_var_t mtmYnT = lp_var_t(sid.objectId, MTM_RAW_Y, this); + lp_var_t mtmZnT = lp_var_t(sid.objectId, MTM_RAW_Z, this); + /** 1 if coils were actuating during measurement otherwise 0 */ + lp_var_t coilActuationStatus = lp_var_t(sid.objectId, ACTUATION_RAW_STATUS, + this); +}; + + /** * @brief This class can be used to ease the generation of an action message commanding the * IMTQHandler to configure the magnettorquer with the desired dipoles. @@ -203,6 +492,542 @@ private: SerializeElement zDipole; SerializeElement duration; }; + +/** + * @brief This dataset can be used to store the self test results of the +X self test. + * + * @details Units of measurements: + * Raw magnetic field: [nT] + * Calibrated magnetic field: [nT] + * Coil currents: [mA] + * Temperature: [°C] + * The +X self test generates a positive dipole in X direction and measures the magnetic + * field with the built-in MTM. The procedure of the test is as follows: + * 1. All coils off (INIT step) + * 2. +X actuation + * 3. All coils off (FINA step) + */ +class PosXSelfTestSet: public StaticLocalDataSet { +public: + + PosXSelfTestSet(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, IMTQ::POS_X_TEST_DATASET) { + } + + PosXSelfTestSet(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, IMTQ::POS_X_TEST_DATASET)) { + } + + /** INIT block */ + lp_var_t initErr = lp_var_t(sid.objectId, INIT_POS_X_ERR, this); + lp_var_t initRawMagX = lp_var_t(sid.objectId, INIT_POS_X_RAW_MAG_X, this); + lp_var_t initRawMagY = lp_var_t(sid.objectId, INIT_POS_X_RAW_MAG_Y, this); + lp_var_t initRawMagZ = lp_var_t(sid.objectId, INIT_POS_X_RAW_MAG_Z, this); + lp_var_t initCalMagX = lp_var_t(sid.objectId, INIT_POS_X_CAL_MAG_X, this); + lp_var_t initCalMagY = lp_var_t(sid.objectId, INIT_POS_X_CAL_MAG_Y, this); + lp_var_t initCalMagZ = lp_var_t(sid.objectId, INIT_POS_X_CAL_MAG_Z, this); + lp_var_t initCoilXCurrent = lp_var_t(sid.objectId, INIT_POS_X_COIL_X_CURRENT, + this); + lp_var_t initCoilYCurrent = lp_var_t(sid.objectId, INIT_POS_X_COIL_Y_CURRENT, + this); + lp_var_t initCoilZCurrent = lp_var_t(sid.objectId, INIT_POS_X_COIL_Z_CURRENT, + this); + lp_var_t initCoilXTemperature = lp_var_t(sid.objectId, + INIT_POS_X_COIL_X_TEMPERATURE, this); + lp_var_t initCoilYTemperature = lp_var_t(sid.objectId, + INIT_POS_X_COIL_Y_TEMPERATURE, this); + lp_var_t initCoilZTemperature = lp_var_t(sid.objectId, + INIT_POS_X_COIL_Z_TEMPERATURE, this); + + /** +X block */ + lp_var_t err = lp_var_t(sid.objectId, POS_X_ERR, this); + lp_var_t rawMagX = lp_var_t(sid.objectId, POS_X_RAW_MAG_X, this); + lp_var_t rawMagY = lp_var_t(sid.objectId, POS_X_RAW_MAG_Y, this); + lp_var_t rawMagZ = lp_var_t(sid.objectId, POS_X_RAW_MAG_Z, this); + lp_var_t calMagX = lp_var_t(sid.objectId, POS_X_CAL_MAG_X, this); + lp_var_t calMagY = lp_var_t(sid.objectId, POS_X_CAL_MAG_Y, this); + lp_var_t calMagZ = lp_var_t(sid.objectId, POS_X_CAL_MAG_Z, this); + lp_var_t coilXCurrent = lp_var_t(sid.objectId, POS_X_COIL_X_CURRENT, + this); + lp_var_t coilYCurrent = lp_var_t(sid.objectId, POS_X_COIL_Y_CURRENT, + this); + lp_var_t coilZCurrent = lp_var_t(sid.objectId, POS_X_COIL_Z_CURRENT, + this); + lp_var_t coilXTemperature = lp_var_t(sid.objectId, + POS_X_COIL_X_TEMPERATURE, this); + lp_var_t coilYTemperature = lp_var_t(sid.objectId, + POS_X_COIL_Y_TEMPERATURE, this); + lp_var_t coilZTemperature = lp_var_t(sid.objectId, + POS_X_COIL_Z_TEMPERATURE, this); + + /** FINA block */ + lp_var_t finaErr = lp_var_t(sid.objectId, FINA_POS_X_ERR, this); + lp_var_t finaRawMagX = lp_var_t(sid.objectId, FINA_POS_X_RAW_MAG_X, this); + lp_var_t finaRawMagY = lp_var_t(sid.objectId, FINA_POS_X_RAW_MAG_Y, this); + lp_var_t finaRawMagZ = lp_var_t(sid.objectId, FINA_POS_X_RAW_MAG_Z, this); + lp_var_t finaCalMagX = lp_var_t(sid.objectId, FINA_POS_X_CAL_MAG_X, this); + lp_var_t finaCalMagY = lp_var_t(sid.objectId, FINA_POS_X_CAL_MAG_Y, this); + lp_var_t finaCalMagZ = lp_var_t(sid.objectId, FINA_POS_X_CAL_MAG_Z, this); + lp_var_t finaCoilXCurrent = lp_var_t(sid.objectId, FINA_POS_X_COIL_X_CURRENT, + this); + lp_var_t finaCoilYCurrent = lp_var_t(sid.objectId, FINA_POS_X_COIL_Y_CURRENT, + this); + lp_var_t finaCoilZCurrent = lp_var_t(sid.objectId, FINA_POS_X_COIL_Z_CURRENT, + this); + lp_var_t finaCoilXTemperature = lp_var_t(sid.objectId, + FINA_POS_X_COIL_X_TEMPERATURE, this); + lp_var_t finaCoilYTemperature = lp_var_t(sid.objectId, + FINA_POS_X_COIL_Y_TEMPERATURE, this); + lp_var_t finaCoilZTemperature = lp_var_t(sid.objectId, + FINA_POS_X_COIL_Z_TEMPERATURE, this); +}; + +/** + * @brief This dataset can be used to store the self test results of the -X self test. + * + * @details Units of measurements: + * Raw magnetic field: [nT] + * Calibrated magnetic field: [nT] + * Coil currents: [mA] + * Temperature: [°C] + * The -X self test generates a negative dipole in X direction and measures the magnetic + * field with the built-in MTM. The procedure of the test is as follows: + * 1. All coils off (INIT step) + * 2. -X actuation + * 3. All coils off (FINA step) + */ +class NegXSelfTestSet: public StaticLocalDataSet { +public: + + NegXSelfTestSet(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, IMTQ::NEG_X_TEST_DATASET) { + } + + NegXSelfTestSet(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, IMTQ::NEG_X_TEST_DATASET)) { + } + + /** INIT block */ + lp_var_t initErr = lp_var_t(sid.objectId, INIT_NEG_X_ERR, this); + lp_var_t initRawMagX = lp_var_t(sid.objectId, INIT_NEG_X_RAW_MAG_X, this); + lp_var_t initRawMagY = lp_var_t(sid.objectId, INIT_NEG_X_RAW_MAG_Y, this); + lp_var_t initRawMagZ = lp_var_t(sid.objectId, INIT_NEG_X_RAW_MAG_Z, this); + lp_var_t initCalMagX = lp_var_t(sid.objectId, INIT_NEG_X_CAL_MAG_X, this); + lp_var_t initCalMagY = lp_var_t(sid.objectId, INIT_NEG_X_CAL_MAG_Y, this); + lp_var_t initCalMagZ = lp_var_t(sid.objectId, INIT_NEG_X_CAL_MAG_Z, this); + lp_var_t initCoilXCurrent = lp_var_t(sid.objectId, INIT_NEG_X_COIL_X_CURRENT, + this); + lp_var_t initCoilYCurrent = lp_var_t(sid.objectId, INIT_NEG_X_COIL_Y_CURRENT, + this); + lp_var_t initCoilZCurrent = lp_var_t(sid.objectId, INIT_NEG_X_COIL_Z_CURRENT, + this); + lp_var_t initCoilXTemperature = lp_var_t(sid.objectId, + INIT_NEG_X_COIL_X_TEMPERATURE, this); + lp_var_t initCoilYTemperature = lp_var_t(sid.objectId, + INIT_NEG_X_COIL_Y_TEMPERATURE, this); + lp_var_t initCoilZTemperature = lp_var_t(sid.objectId, + INIT_NEG_X_COIL_Z_TEMPERATURE, this); + + /** -X block */ + lp_var_t err = lp_var_t(sid.objectId, NEG_X_ERR, this); + lp_var_t rawMagX = lp_var_t(sid.objectId, NEG_X_RAW_MAG_X, this); + lp_var_t rawMagY = lp_var_t(sid.objectId, NEG_X_RAW_MAG_Y, this); + lp_var_t rawMagZ = lp_var_t(sid.objectId, NEG_X_RAW_MAG_Z, this); + lp_var_t calMagX = lp_var_t(sid.objectId, NEG_X_CAL_MAG_X, this); + lp_var_t calMagY = lp_var_t(sid.objectId, NEG_X_CAL_MAG_Y, this); + lp_var_t calMagZ = lp_var_t(sid.objectId, NEG_X_CAL_MAG_Z, this); + lp_var_t coilXCurrent = lp_var_t(sid.objectId, NEG_X_COIL_X_CURRENT, + this); + lp_var_t coilYCurrent = lp_var_t(sid.objectId, NEG_X_COIL_Y_CURRENT, + this); + lp_var_t coilZCurrent = lp_var_t(sid.objectId, NEG_X_COIL_Z_CURRENT, + this); + lp_var_t coilXTemperature = lp_var_t(sid.objectId, + NEG_X_COIL_X_TEMPERATURE, this); + lp_var_t coilYTemperature = lp_var_t(sid.objectId, + NEG_X_COIL_Y_TEMPERATURE, this); + lp_var_t coilZTemperature = lp_var_t(sid.objectId, + NEG_X_COIL_Z_TEMPERATURE, this); + + /** FINA block */ + lp_var_t finaErr = lp_var_t(sid.objectId, FINA_NEG_X_ERR, this); + lp_var_t finaRawMagX = lp_var_t(sid.objectId, FINA_NEG_X_RAW_MAG_X, this); + lp_var_t finaRawMagY = lp_var_t(sid.objectId, FINA_NEG_X_RAW_MAG_Y, this); + lp_var_t finaRawMagZ = lp_var_t(sid.objectId, FINA_NEG_X_RAW_MAG_Z, this); + lp_var_t finaCalMagX = lp_var_t(sid.objectId, FINA_NEG_X_CAL_MAG_X, this); + lp_var_t finaCalMagY = lp_var_t(sid.objectId, FINA_NEG_X_CAL_MAG_Y, this); + lp_var_t finaCalMagZ = lp_var_t(sid.objectId, FINA_NEG_X_CAL_MAG_Z, this); + lp_var_t finaCoilXCurrent = lp_var_t(sid.objectId, FINA_NEG_X_COIL_X_CURRENT, + this); + lp_var_t finaCoilYCurrent = lp_var_t(sid.objectId, FINA_NEG_X_COIL_Y_CURRENT, + this); + lp_var_t finaCoilZCurrent = lp_var_t(sid.objectId, FINA_NEG_X_COIL_Z_CURRENT, + this); + lp_var_t finaCoilXTemperature = lp_var_t(sid.objectId, + FINA_NEG_X_COIL_X_TEMPERATURE, this); + lp_var_t finaCoilYTemperature = lp_var_t(sid.objectId, + FINA_NEG_X_COIL_Y_TEMPERATURE, this); + lp_var_t finaCoilZTemperature = lp_var_t(sid.objectId, + FINA_NEG_X_COIL_Z_TEMPERATURE, this); +}; + + +/** + * @brief This dataset can be used to store the self test results of the +Y self test. + * + * @details Units of measurements: + * Raw magnetic field: [nT] + * Calibrated magnetic field: [nT] + * Coil currents: [mA] + * Temperature: [°C] + * The +Y self test generates a positive dipole in y direction and measures the magnetic + * field with the built-in MTM. The procedure of the test is as follows: + * 1. All coils off (INIT step) + * 2. +Y actuation + * 3. All coils off (FINA step) + */ +class PosYSelfTestSet: public StaticLocalDataSet { +public: + + PosYSelfTestSet(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, IMTQ::POS_Y_TEST_DATASET) { + } + + PosYSelfTestSet(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, IMTQ::POS_Y_TEST_DATASET)) { + } + + /** INIT block */ + lp_var_t initErr = lp_var_t(sid.objectId, INIT_POS_Y_ERR, this); + lp_var_t initRawMagX = lp_var_t(sid.objectId, INIT_POS_Y_RAW_MAG_X, this); + lp_var_t initRawMagY = lp_var_t(sid.objectId, INIT_POS_Y_RAW_MAG_Y, this); + lp_var_t initRawMagZ = lp_var_t(sid.objectId, INIT_POS_Y_RAW_MAG_Z, this); + lp_var_t initCalMagX = lp_var_t(sid.objectId, INIT_POS_Y_CAL_MAG_X, this); + lp_var_t initCalMagY = lp_var_t(sid.objectId, INIT_POS_Y_CAL_MAG_Y, this); + lp_var_t initCalMagZ = lp_var_t(sid.objectId, INIT_POS_Y_CAL_MAG_Z, this); + lp_var_t initCoilXCurrent = lp_var_t(sid.objectId, INIT_POS_Y_COIL_X_CURRENT, + this); + lp_var_t initCoilYCurrent = lp_var_t(sid.objectId, INIT_POS_Y_COIL_Y_CURRENT, + this); + lp_var_t initCoilZCurrent = lp_var_t(sid.objectId, INIT_POS_Y_COIL_Z_CURRENT, + this); + lp_var_t initCoilXTemperature = lp_var_t(sid.objectId, + INIT_POS_Y_COIL_X_TEMPERATURE, this); + lp_var_t initCoilYTemperature = lp_var_t(sid.objectId, + INIT_POS_Y_COIL_Y_TEMPERATURE, this); + lp_var_t initCoilZTemperature = lp_var_t(sid.objectId, + INIT_POS_Y_COIL_Z_TEMPERATURE, this); + + /** +Y block */ + lp_var_t err = lp_var_t(sid.objectId, POS_Y_ERR, this); + lp_var_t rawMagX = lp_var_t(sid.objectId, POS_Y_RAW_MAG_X, this); + lp_var_t rawMagY = lp_var_t(sid.objectId, POS_Y_RAW_MAG_Y, this); + lp_var_t rawMagZ = lp_var_t(sid.objectId, POS_Y_RAW_MAG_Z, this); + lp_var_t calMagX = lp_var_t(sid.objectId, POS_Y_CAL_MAG_X, this); + lp_var_t calMagY = lp_var_t(sid.objectId, POS_Y_CAL_MAG_Y, this); + lp_var_t calMagZ = lp_var_t(sid.objectId, POS_Y_CAL_MAG_Z, this); + lp_var_t coilXCurrent = lp_var_t(sid.objectId, POS_Y_COIL_X_CURRENT, + this); + lp_var_t coilYCurrent = lp_var_t(sid.objectId, POS_Y_COIL_Y_CURRENT, + this); + lp_var_t coilZCurrent = lp_var_t(sid.objectId, POS_Y_COIL_Z_CURRENT, + this); + lp_var_t coilXTemperature = lp_var_t(sid.objectId, + POS_Y_COIL_X_TEMPERATURE, this); + lp_var_t coilYTemperature = lp_var_t(sid.objectId, + POS_Y_COIL_Y_TEMPERATURE, this); + lp_var_t coilZTemperature = lp_var_t(sid.objectId, + POS_Y_COIL_Z_TEMPERATURE, this); + + /** FINA block */ + lp_var_t finaErr = lp_var_t(sid.objectId, FINA_POS_Y_ERR, this); + lp_var_t finaRawMagX = lp_var_t(sid.objectId, FINA_POS_Y_RAW_MAG_X, this); + lp_var_t finaRawMagY = lp_var_t(sid.objectId, FINA_POS_Y_RAW_MAG_Y, this); + lp_var_t finaRawMagZ = lp_var_t(sid.objectId, FINA_POS_Y_RAW_MAG_Z, this); + lp_var_t finaCalMagX = lp_var_t(sid.objectId, FINA_POS_Y_CAL_MAG_X, this); + lp_var_t finaCalMagY = lp_var_t(sid.objectId, FINA_POS_Y_CAL_MAG_Y, this); + lp_var_t finaCalMagZ = lp_var_t(sid.objectId, FINA_POS_Y_CAL_MAG_Z, this); + lp_var_t finaCoilXCurrent = lp_var_t(sid.objectId, FINA_POS_Y_COIL_X_CURRENT, + this); + lp_var_t finaCoilYCurrent = lp_var_t(sid.objectId, FINA_POS_Y_COIL_Y_CURRENT, + this); + lp_var_t finaCoilZCurrent = lp_var_t(sid.objectId, FINA_POS_Y_COIL_Z_CURRENT, + this); + lp_var_t finaCoilXTemperature = lp_var_t(sid.objectId, + FINA_POS_Y_COIL_X_TEMPERATURE, this); + lp_var_t finaCoilYTemperature = lp_var_t(sid.objectId, + FINA_POS_Y_COIL_Y_TEMPERATURE, this); + lp_var_t finaCoilZTemperature = lp_var_t(sid.objectId, + FINA_POS_Y_COIL_Z_TEMPERATURE, this); +}; + +/** + * @brief This dataset can be used to store the self test results of the -Y self test. + * + * @details Units of measurements: + * Raw magnetic field: [nT] + * Calibrated magnetic field: [nT] + * Coil currents: [mA] + * Temperature: [°C] + * The -Y self test generates a negative dipole in y direction and measures the magnetic + * field with the built-in MTM. The procedure of the test is as follows: + * 1. All coils off (INIT step) + * 2. -Y actuation + * 3. All coils off (FINA step) + */ +class NegYSelfTestSet: public StaticLocalDataSet { +public: + + NegYSelfTestSet(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, IMTQ::NEG_Y_TEST_DATASET) { + } + + NegYSelfTestSet(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, IMTQ::NEG_Y_TEST_DATASET)) { + } + + /** INIT block */ + lp_var_t initErr = lp_var_t(sid.objectId, INIT_NEG_Y_ERR, this); + lp_var_t initRawMagX = lp_var_t(sid.objectId, INIT_NEG_Y_RAW_MAG_X, this); + lp_var_t initRawMagY = lp_var_t(sid.objectId, INIT_NEG_Y_RAW_MAG_Y, this); + lp_var_t initRawMagZ = lp_var_t(sid.objectId, INIT_NEG_Y_RAW_MAG_Z, this); + lp_var_t initCalMagX = lp_var_t(sid.objectId, INIT_NEG_Y_CAL_MAG_X, this); + lp_var_t initCalMagY = lp_var_t(sid.objectId, INIT_NEG_Y_CAL_MAG_Y, this); + lp_var_t initCalMagZ = lp_var_t(sid.objectId, INIT_NEG_Y_CAL_MAG_Z, this); + lp_var_t initCoilXCurrent = lp_var_t(sid.objectId, INIT_NEG_Y_COIL_X_CURRENT, + this); + lp_var_t initCoilYCurrent = lp_var_t(sid.objectId, INIT_NEG_Y_COIL_Y_CURRENT, + this); + lp_var_t initCoilZCurrent = lp_var_t(sid.objectId, INIT_NEG_Y_COIL_Z_CURRENT, + this); + lp_var_t initCoilXTemperature = lp_var_t(sid.objectId, + INIT_NEG_Y_COIL_X_TEMPERATURE, this); + lp_var_t initCoilYTemperature = lp_var_t(sid.objectId, + INIT_NEG_Y_COIL_Y_TEMPERATURE, this); + lp_var_t initCoilZTemperature = lp_var_t(sid.objectId, + INIT_NEG_Y_COIL_Z_TEMPERATURE, this); + + /** -Y block */ + lp_var_t err = lp_var_t(sid.objectId, NEG_Y_ERR, this); + lp_var_t rawMagX = lp_var_t(sid.objectId, NEG_Y_RAW_MAG_X, this); + lp_var_t rawMagY = lp_var_t(sid.objectId, NEG_Y_RAW_MAG_Y, this); + lp_var_t rawMagZ = lp_var_t(sid.objectId, NEG_Y_RAW_MAG_Z, this); + lp_var_t calMagX = lp_var_t(sid.objectId, NEG_Y_CAL_MAG_X, this); + lp_var_t calMagY = lp_var_t(sid.objectId, NEG_Y_CAL_MAG_Y, this); + lp_var_t calMagZ = lp_var_t(sid.objectId, NEG_Y_CAL_MAG_Z, this); + lp_var_t coilXCurrent = lp_var_t(sid.objectId, NEG_Y_COIL_X_CURRENT, + this); + lp_var_t coilYCurrent = lp_var_t(sid.objectId, NEG_Y_COIL_Y_CURRENT, + this); + lp_var_t coilZCurrent = lp_var_t(sid.objectId, NEG_Y_COIL_Z_CURRENT, + this); + lp_var_t coilXTemperature = lp_var_t(sid.objectId, + NEG_Y_COIL_X_TEMPERATURE, this); + lp_var_t coilYTemperature = lp_var_t(sid.objectId, + NEG_Y_COIL_Y_TEMPERATURE, this); + lp_var_t coilZTemperature = lp_var_t(sid.objectId, + NEG_Y_COIL_Z_TEMPERATURE, this); + + /** FINA block */ + lp_var_t finaErr = lp_var_t(sid.objectId, FINA_NEG_Y_ERR, this); + lp_var_t finaRawMagX = lp_var_t(sid.objectId, FINA_NEG_Y_RAW_MAG_X, this); + lp_var_t finaRawMagY = lp_var_t(sid.objectId, FINA_NEG_Y_RAW_MAG_Y, this); + lp_var_t finaRawMagZ = lp_var_t(sid.objectId, FINA_NEG_Y_RAW_MAG_Z, this); + lp_var_t finaCalMagX = lp_var_t(sid.objectId, FINA_NEG_Y_CAL_MAG_X, this); + lp_var_t finaCalMagY = lp_var_t(sid.objectId, FINA_NEG_Y_CAL_MAG_Y, this); + lp_var_t finaCalMagZ = lp_var_t(sid.objectId, FINA_NEG_Y_CAL_MAG_Z, this); + lp_var_t finaCoilXCurrent = lp_var_t(sid.objectId, FINA_NEG_Y_COIL_X_CURRENT, + this); + lp_var_t finaCoilYCurrent = lp_var_t(sid.objectId, FINA_NEG_Y_COIL_Y_CURRENT, + this); + lp_var_t finaCoilZCurrent = lp_var_t(sid.objectId, FINA_NEG_Y_COIL_Z_CURRENT, + this); + lp_var_t finaCoilXTemperature = lp_var_t(sid.objectId, + FINA_NEG_Y_COIL_X_TEMPERATURE, this); + lp_var_t finaCoilYTemperature = lp_var_t(sid.objectId, + FINA_NEG_Y_COIL_Y_TEMPERATURE, this); + lp_var_t finaCoilZTemperature = lp_var_t(sid.objectId, + FINA_NEG_Y_COIL_Z_TEMPERATURE, this); +}; + +/** + * @brief This dataset can be used to store the self test results of the +Z self test. + * + * @details Units of measurements: + * Raw magnetic field: [nT] + * Calibrated magnetic field: [nT] + * Coil currents: [mA] + * Temperature: [°C] + * The +Z self test generates a positive dipole in z direction and measures the magnetic + * field with the built-in MTM. The procedure of the test is as follows: + * 1. All coils off (INIT step) + * 2. +Z actuation + * 3. All coils off (FINA step) + */ +class PosZSelfTestSet: public StaticLocalDataSet { +public: + + PosZSelfTestSet(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, IMTQ::POS_Z_TEST_DATASET) { + } + + PosZSelfTestSet(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, IMTQ::POS_Z_TEST_DATASET)) { + } + + /** INIT block */ + lp_var_t initErr = lp_var_t(sid.objectId, INIT_POS_Z_ERR, this); + lp_var_t initRawMagX = lp_var_t(sid.objectId, INIT_POS_Z_RAW_MAG_X, this); + lp_var_t initRawMagY = lp_var_t(sid.objectId, INIT_POS_Z_RAW_MAG_Y, this); + lp_var_t initRawMagZ = lp_var_t(sid.objectId, INIT_POS_Z_RAW_MAG_Z, this); + lp_var_t initCalMagX = lp_var_t(sid.objectId, INIT_POS_Z_CAL_MAG_X, this); + lp_var_t initCalMagY = lp_var_t(sid.objectId, INIT_POS_Z_CAL_MAG_Y, this); + lp_var_t initCalMagZ = lp_var_t(sid.objectId, INIT_POS_Z_CAL_MAG_Z, this); + lp_var_t initCoilXCurrent = lp_var_t(sid.objectId, INIT_POS_Z_COIL_X_CURRENT, + this); + lp_var_t initCoilYCurrent = lp_var_t(sid.objectId, INIT_POS_Z_COIL_Y_CURRENT, + this); + lp_var_t initCoilZCurrent = lp_var_t(sid.objectId, INIT_POS_Z_COIL_Z_CURRENT, + this); + lp_var_t initCoilXTemperature = lp_var_t(sid.objectId, + INIT_POS_Z_COIL_X_TEMPERATURE, this); + lp_var_t initCoilYTemperature = lp_var_t(sid.objectId, + INIT_POS_Z_COIL_Y_TEMPERATURE, this); + lp_var_t initCoilZTemperature = lp_var_t(sid.objectId, + INIT_POS_Z_COIL_Z_TEMPERATURE, this); + + /** +Z block */ + lp_var_t err = lp_var_t(sid.objectId, POS_Z_ERR, this); + lp_var_t rawMagX = lp_var_t(sid.objectId, POS_Z_RAW_MAG_X, this); + lp_var_t rawMagY = lp_var_t(sid.objectId, POS_Z_RAW_MAG_Y, this); + lp_var_t rawMagZ = lp_var_t(sid.objectId, POS_Z_RAW_MAG_Z, this); + lp_var_t calMagX = lp_var_t(sid.objectId, POS_Z_CAL_MAG_X, this); + lp_var_t calMagY = lp_var_t(sid.objectId, POS_Z_CAL_MAG_Y, this); + lp_var_t calMagZ = lp_var_t(sid.objectId, POS_Z_CAL_MAG_Z, this); + lp_var_t coilXCurrent = lp_var_t(sid.objectId, POS_Z_COIL_X_CURRENT, + this); + lp_var_t coilYCurrent = lp_var_t(sid.objectId, POS_Z_COIL_Y_CURRENT, + this); + lp_var_t coilZCurrent = lp_var_t(sid.objectId, POS_Z_COIL_Z_CURRENT, + this); + lp_var_t coilXTemperature = lp_var_t(sid.objectId, + POS_Z_COIL_X_TEMPERATURE, this); + lp_var_t coilYTemperature = lp_var_t(sid.objectId, + POS_Z_COIL_Y_TEMPERATURE, this); + lp_var_t coilZTemperature = lp_var_t(sid.objectId, + POS_Z_COIL_Z_TEMPERATURE, this); + + /** FINA block */ + lp_var_t finaErr = lp_var_t(sid.objectId, FINA_POS_Z_ERR, this); + lp_var_t finaRawMagX = lp_var_t(sid.objectId, FINA_POS_Z_RAW_MAG_X, this); + lp_var_t finaRawMagY = lp_var_t(sid.objectId, FINA_POS_Z_RAW_MAG_Y, this); + lp_var_t finaRawMagZ = lp_var_t(sid.objectId, FINA_POS_Z_RAW_MAG_Z, this); + lp_var_t finaCalMagX = lp_var_t(sid.objectId, FINA_POS_Z_CAL_MAG_X, this); + lp_var_t finaCalMagY = lp_var_t(sid.objectId, FINA_POS_Z_CAL_MAG_Y, this); + lp_var_t finaCalMagZ = lp_var_t(sid.objectId, FINA_POS_Z_CAL_MAG_Z, this); + lp_var_t finaCoilXCurrent = lp_var_t(sid.objectId, FINA_POS_Z_COIL_X_CURRENT, + this); + lp_var_t finaCoilYCurrent = lp_var_t(sid.objectId, FINA_POS_Z_COIL_Y_CURRENT, + this); + lp_var_t finaCoilZCurrent = lp_var_t(sid.objectId, FINA_POS_Z_COIL_Z_CURRENT, + this); + lp_var_t finaCoilXTemperature = lp_var_t(sid.objectId, + FINA_POS_Z_COIL_X_TEMPERATURE, this); + lp_var_t finaCoilYTemperature = lp_var_t(sid.objectId, + FINA_POS_Z_COIL_Y_TEMPERATURE, this); + lp_var_t finaCoilZTemperature = lp_var_t(sid.objectId, + FINA_POS_Z_COIL_Z_TEMPERATURE, this); +}; + +/** + * @brief This dataset can be used to store the self test results of the -Z self test. + * + * @details Units of measurements: + * Raw magnetic field: [nT] + * Calibrated magnetic field: [nT] + * Coil currents: [mA] + * Temperature: [°C] + * The -Z self test generates a negative dipole in z direction and measures the magnetic + * field with the built-in MTM. The procedure of the test is as follows: + * 1. All coils off (INIT step) + * 2. -Z actuation + * 3. All coils off (FINA step) + */ +class NegZSelfTestSet: public StaticLocalDataSet { +public: + + NegZSelfTestSet(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, IMTQ::NEG_Z_TEST_DATASET) { + } + + NegZSelfTestSet(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, IMTQ::NEG_Z_TEST_DATASET)) { + } + + /** INIT block */ + lp_var_t initErr = lp_var_t(sid.objectId, INIT_NEG_Z_ERR, this); + lp_var_t initRawMagX = lp_var_t(sid.objectId, INIT_NEG_Z_RAW_MAG_X, this); + lp_var_t initRawMagY = lp_var_t(sid.objectId, INIT_NEG_Z_RAW_MAG_Y, this); + lp_var_t initRawMagZ = lp_var_t(sid.objectId, INIT_NEG_Z_RAW_MAG_Z, this); + lp_var_t initCalMagX = lp_var_t(sid.objectId, INIT_NEG_Z_CAL_MAG_X, this); + lp_var_t initCalMagY = lp_var_t(sid.objectId, INIT_NEG_Z_CAL_MAG_Y, this); + lp_var_t initCalMagZ = lp_var_t(sid.objectId, INIT_NEG_Z_CAL_MAG_Z, this); + lp_var_t initCoilXCurrent = lp_var_t(sid.objectId, INIT_NEG_Z_COIL_X_CURRENT, + this); + lp_var_t initCoilYCurrent = lp_var_t(sid.objectId, INIT_NEG_Z_COIL_Y_CURRENT, + this); + lp_var_t initCoilZCurrent = lp_var_t(sid.objectId, INIT_NEG_Z_COIL_Z_CURRENT, + this); + lp_var_t initCoilXTemperature = lp_var_t(sid.objectId, + INIT_NEG_Z_COIL_X_TEMPERATURE, this); + lp_var_t initCoilYTemperature = lp_var_t(sid.objectId, + INIT_NEG_Z_COIL_Y_TEMPERATURE, this); + lp_var_t initCoilZTemperature = lp_var_t(sid.objectId, + INIT_NEG_Z_COIL_Z_TEMPERATURE, this); + + /** +Z block */ + lp_var_t err = lp_var_t(sid.objectId, NEG_Z_ERR, this); + lp_var_t rawMagX = lp_var_t(sid.objectId, NEG_Z_RAW_MAG_X, this); + lp_var_t rawMagY = lp_var_t(sid.objectId, NEG_Z_RAW_MAG_Y, this); + lp_var_t rawMagZ = lp_var_t(sid.objectId, NEG_Z_RAW_MAG_Z, this); + lp_var_t calMagX = lp_var_t(sid.objectId, NEG_Z_CAL_MAG_X, this); + lp_var_t calMagY = lp_var_t(sid.objectId, NEG_Z_CAL_MAG_Y, this); + lp_var_t calMagZ = lp_var_t(sid.objectId, NEG_Z_CAL_MAG_Z, this); + lp_var_t coilXCurrent = lp_var_t(sid.objectId, NEG_Z_COIL_X_CURRENT, + this); + lp_var_t coilYCurrent = lp_var_t(sid.objectId, NEG_Z_COIL_Y_CURRENT, + this); + lp_var_t coilZCurrent = lp_var_t(sid.objectId, NEG_Z_COIL_Z_CURRENT, + this); + lp_var_t coilXTemperature = lp_var_t(sid.objectId, + NEG_Z_COIL_X_TEMPERATURE, this); + lp_var_t coilYTemperature = lp_var_t(sid.objectId, + NEG_Z_COIL_Y_TEMPERATURE, this); + lp_var_t coilZTemperature = lp_var_t(sid.objectId, + NEG_Z_COIL_Z_TEMPERATURE, this); + + /** FINA block */ + lp_var_t finaErr = lp_var_t(sid.objectId, FINA_NEG_Z_ERR, this); + lp_var_t finaRawMagX = lp_var_t(sid.objectId, FINA_NEG_Z_RAW_MAG_X, this); + lp_var_t finaRawMagY = lp_var_t(sid.objectId, FINA_NEG_Z_RAW_MAG_Y, this); + lp_var_t finaRawMagZ = lp_var_t(sid.objectId, FINA_NEG_Z_RAW_MAG_Z, this); + lp_var_t finaCalMagX = lp_var_t(sid.objectId, FINA_NEG_Z_CAL_MAG_X, this); + lp_var_t finaCalMagY = lp_var_t(sid.objectId, FINA_NEG_Z_CAL_MAG_Y, this); + lp_var_t finaCalMagZ = lp_var_t(sid.objectId, FINA_NEG_Z_CAL_MAG_Z, this); + lp_var_t finaCoilXCurrent = lp_var_t(sid.objectId, FINA_NEG_Z_COIL_X_CURRENT, + this); + lp_var_t finaCoilYCurrent = lp_var_t(sid.objectId, FINA_NEG_Z_COIL_Y_CURRENT, + this); + lp_var_t finaCoilZCurrent = lp_var_t(sid.objectId, FINA_NEG_Z_COIL_Z_CURRENT, + this); + lp_var_t finaCoilXTemperature = lp_var_t(sid.objectId, + FINA_NEG_Z_COIL_X_TEMPERATURE, this); + lp_var_t finaCoilYTemperature = lp_var_t(sid.objectId, + FINA_NEG_Z_COIL_Y_TEMPERATURE, this); + lp_var_t finaCoilZTemperature = lp_var_t(sid.objectId, + FINA_NEG_Z_COIL_Z_TEMPERATURE, this); +}; + } diff --git a/mission/devices/devicedefinitions/Max31865Definitions.h b/mission/devices/devicedefinitions/Max31865Definitions.h index bdf82dc8..2a789a7b 100644 --- a/mission/devices/devicedefinitions/Max31865Definitions.h +++ b/mission/devices/devicedefinitions/Max31865Definitions.h @@ -14,8 +14,12 @@ enum PoolIds: lp_id_t { }; static constexpr DeviceCommandId_t CONFIG_CMD = 0x80; +static constexpr DeviceCommandId_t WRITE_HIGH_THRESHOLD = 0x83; +static constexpr DeviceCommandId_t WRITE_LOW_THRESHOLD = 0x85; static constexpr DeviceCommandId_t REQUEST_CONFIG = 0x00; static constexpr DeviceCommandId_t REQUEST_RTD = 0x01; +static constexpr DeviceCommandId_t REQUEST_HIGH_THRESHOLD = 0x03; +static constexpr DeviceCommandId_t REQUEST_LOW_THRESHOLD = 0x05; static constexpr DeviceCommandId_t REQUEST_FAULT_BYTE = 0x07; static constexpr uint32_t MAX31865_SET_ID = REQUEST_RTD; diff --git a/tmtc b/tmtc index 8fc67c26..20a44b4b 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 8fc67c26653df03f3352493bd0c744bc79928e1d +Subproject commit 20a44b4b67198d42e83a790191ad6740d0aabf7c