diff --git a/CHANGELOG.md b/CHANGELOG.md index bead435b..41b24103 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -28,6 +28,13 @@ change warranting a new major release: ## Changed - Add `-Wshadow=local` shadowing warnings and fixed all of them +- Refactored IMTQ handlers to also perform low level I2C communication tasks in separate thread. + This avoids the various delays needed for I2C communication with that device inside the ACS PST. + (e.g. 1 ms delay between each transfer, or 10 ms integration delay for MGM measurements). +- Updated generated CSV files: Support for skip directive and explicit + "No description" info string +- The polling threads for actuator polling now have a slightly higher priority than the ACS PST + to ensure timing requirements are met. ## Added diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp index 44dc754a..71f28a31 100644 --- a/bsp_hosted/fsfwconfig/events/translateEvents.cpp +++ b/bsp_hosted/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** * @brief Auto-generated event translation file. Contains 257 translations. * @details - * Generated on: 2023-02-21 00:24:44 + * Generated on: 2023-02-21 10:44:59 */ #include "translateEvents.h" diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp index 1600ee48..08b7ac62 100644 --- a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp +++ b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp @@ -1,8 +1,8 @@ /** * @brief Auto-generated object translation file. * @details - * Contains 147 translations. - * Generated on: 2023-02-21 00:24:44 + * Contains 148 translations. + * Generated on: 2023-02-21 10:44:59 */ #include "translateObjects.h" @@ -38,6 +38,7 @@ const char *GYRO_3_L3G_HANDLER_STRING = "GYRO_3_L3G_HANDLER"; const char *RW4_STRING = "RW4"; const char *STAR_TRACKER_STRING = "STAR_TRACKER"; const char *GPS_CONTROLLER_STRING = "GPS_CONTROLLER"; +const char *IMTQ_POLLING_STRING = "IMTQ_POLLING"; const char *IMTQ_HANDLER_STRING = "IMTQ_HANDLER"; const char *PCDU_HANDLER_STRING = "PCDU_HANDLER"; const char *P60DOCK_HANDLER_STRING = "P60DOCK_HANDLER"; @@ -220,6 +221,8 @@ const char *translateObject(object_id_t object) { return STAR_TRACKER_STRING; case 0x44130045: return GPS_CONTROLLER_STRING; + case 0x44140013: + return IMTQ_POLLING_STRING; case 0x44140014: return IMTQ_HANDLER_STRING; case 0x442000A1: diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index e2abb9c5..4c22f7f5 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -1,6 +1,7 @@ #include "ObjectFactory.h" #include +#include #include #include @@ -906,8 +907,9 @@ void ObjectFactory::createStrComponents(PowerSwitchIF* pwrSwitcher) { } void ObjectFactory::createImtqComponents(PowerSwitchIF* pwrSwitcher) { - I2cCookie* imtqI2cCookie = new I2cCookie(addresses::IMTQ, IMTQ::MAX_REPLY_SIZE, q7s::I2C_PL_EIVE); - auto imtqHandler = new ImtqHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie, + new ImtqPollingTask(objects::IMTQ_POLLING); + I2cCookie* imtqI2cCookie = new I2cCookie(addresses::IMTQ, imtq::MAX_REPLY_SIZE, q7s::I2C_PL_EIVE); + auto imtqHandler = new ImtqHandler(objects::IMTQ_HANDLER, objects::IMTQ_POLLING, imtqI2cCookie, pcdu::Switches::PDU1_CH3_MGT_5V); imtqHandler->enableThermalModule(ThermalStateCfg()); imtqHandler->setPowerSwitcher(pwrSwitcher); diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index 9d96184b..3c3695dd 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -198,12 +198,20 @@ void scheduling::initTasks() { #if OBSW_ADD_RW == 1 PeriodicTaskIF* rwPolling = factory->createPeriodicTask( - "RW_POLLING_TASK", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); + "RW_POLLING_TASK", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); result = rwPolling->addComponent(objects::RW_POLLING_TASK); if (result != returnvalue::OK) { scheduling::printAddObjectError("RW_POLLING_TASK", objects::RW_POLLING_TASK); } #endif +#if OBSW_ADD_MGT == 1 + PeriodicTaskIF* imtqPolling = factory->createPeriodicTask( + "IMTQ_POLLING_TASK", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); + result = imtqPolling->addComponent(objects::IMTQ_POLLING); + if (result != returnvalue::OK) { + scheduling::printAddObjectError("IMTQ_POLLING_TASK", objects::IMTQ_POLLING); + } +#endif PeriodicTaskIF* acsSysTask = factory->createPeriodicTask( "ACS_SYS_TASK", 55, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); @@ -345,6 +353,9 @@ void scheduling::initTasks() { #if OBSW_ADD_SA_DEPL == 1 solarArrayDeplTask->startTask(); #endif +#if OBSW_ADD_MGT == 1 + imtqPolling->startTask(); +#endif taskStarter(pstTasks, "PST task vector"); taskStarter(pusTasks, "PUS task vector"); @@ -396,7 +407,7 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction static constexpr float acsPstPeriod = 0.8; #endif FixedTimeslotTaskIF* acsTcsPst = factory.createFixedTimeslotTask( - "ACS_TCS_PST", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, acsPstPeriod, missedDeadlineFunc); + "ACS_TCS_PST", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, acsPstPeriod, missedDeadlineFunc); result = pst::pstTcsAndAcs(acsTcsPst, cfg); if (result != returnvalue::OK) { if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { diff --git a/common/config/eive/definitions.h b/common/config/eive/definitions.h index 202bd89d..c87c89ff 100644 --- a/common/config/eive/definitions.h +++ b/common/config/eive/definitions.h @@ -59,17 +59,23 @@ namespace acs { static constexpr uint32_t SCHED_BLOCK_1_SUS_READ_MS = 15; static constexpr uint32_t SCHED_BLOCK_2_SENSOR_READ_MS = 30; -static constexpr uint32_t SCHED_BLOCK_3_ACS_CTRL_MS = 45; -static constexpr uint32_t SCHED_BLOCK_4_ACTUATOR_MS = 50; -static constexpr uint32_t SCHED_BLOCK_5_RW_READ_MS = 300; +static constexpr uint32_t SCHED_BLOCK_3_READ_IMTQ_MGM_MS = 42; +static constexpr uint32_t SCHED_BLOCK_4_ACS_CTRL_MS = 45; +static constexpr uint32_t SCHED_BLOCK_5_ACTUATOR_MS = 50; +static constexpr uint32_t SCHED_BLOCK_6_IMTQ_BLOCK_2_MS = 75; +static constexpr uint32_t SCHED_BLOCK_7_RW_READ_MS = 300; // 15 ms for FM static constexpr float SCHED_BLOCK_1_PERIOD = static_cast(SCHED_BLOCK_1_SUS_READ_MS) / 400.0; static constexpr float SCHED_BLOCK_2_PERIOD = static_cast(SCHED_BLOCK_2_SENSOR_READ_MS) / 400.0; -static constexpr float SCHED_BLOCK_3_PERIOD = static_cast(SCHED_BLOCK_3_ACS_CTRL_MS) / 400.0; -static constexpr float SCHED_BLOCK_4_PERIOD = static_cast(SCHED_BLOCK_4_ACTUATOR_MS) / 400.0; -static constexpr float SCHED_BLOCK_5_PERIOD = static_cast(SCHED_BLOCK_5_RW_READ_MS) / 400.0; +static constexpr float SCHED_BLOCK_3_PERIOD = + static_cast(SCHED_BLOCK_3_READ_IMTQ_MGM_MS) / 400.0; +static constexpr float SCHED_BLOCK_4_PERIOD = static_cast(SCHED_BLOCK_4_ACS_CTRL_MS) / 400.0; +static constexpr float SCHED_BLOCK_5_PERIOD = static_cast(SCHED_BLOCK_5_ACTUATOR_MS) / 400.0; +static constexpr float SCHED_BLOCK_6_PERIOD = + static_cast(SCHED_BLOCK_6_IMTQ_BLOCK_2_MS) / 400.0; +static constexpr float SCHED_BLOCK_7_PERIOD = static_cast(SCHED_BLOCK_7_RW_READ_MS) / 400.0; } // namespace acs diff --git a/common/config/eive/objects.h b/common/config/eive/objects.h index 9bd6099d..105c78cc 100644 --- a/common/config/eive/objects.h +++ b/common/config/eive/objects.h @@ -44,6 +44,7 @@ enum commonObjects : uint32_t { STAR_TRACKER = 0x44130001, GPS_CONTROLLER = 0x44130045, + IMTQ_POLLING = 0x44140013, IMTQ_HANDLER = 0x44140014, TMP1075_HANDLER_TCS_0 = 0x44420004, TMP1075_HANDLER_TCS_1 = 0x44420005, diff --git a/dummies/ImtqDummy.cpp b/dummies/ImtqDummy.cpp index 44a7e377..9fcca838 100644 --- a/dummies/ImtqDummy.cpp +++ b/dummies/ImtqDummy.cpp @@ -1,6 +1,6 @@ #include "ImtqDummy.h" -#include +#include ImtqDummy::ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) : DeviceHandlerBase(objectId, comif, comCookie) {} @@ -38,10 +38,10 @@ uint32_t ImtqDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { retur ReturnValue_t ImtqDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry({0})); - localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, new PoolEntry({0.0, 0.0, 0.0})); - localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry({0})); - localDataPoolMap.emplace(IMTQ::MTM_RAW, new PoolEntry({0.12, 0.76, -0.45}, true)); - localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::MCU_TEMPERATURE, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::MGM_CAL_NT, new PoolEntry({0.0, 0.0, 0.0})); + localDataPoolMap.emplace(imtq::ACTUATION_CAL_STATUS, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::MTM_RAW, new PoolEntry({0.12, 0.76, -0.45}, true)); + localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS, new PoolEntry({0})); return DeviceHandlerBase::initializeLocalDataPool(localDataPoolMap, poolManager); } diff --git a/fsfw b/fsfw index c3279852..2efff4d2 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit c32798522283d89028b7c1ecd3bd33b8391e1a39 +Subproject commit 2efff4d2c5ef82b5b62567ab1bb0ee53aeed6a5a diff --git a/generators/bsp_hosted_objects.csv b/generators/bsp_hosted_objects.csv index 59710edb..d4c5ca74 100644 --- a/generators/bsp_hosted_objects.csv +++ b/generators/bsp_hosted_objects.csv @@ -30,6 +30,7 @@ 0x44120350;RW4 0x44130001;STAR_TRACKER 0x44130045;GPS_CONTROLLER +0x44140013;IMTQ_POLLING 0x44140014;IMTQ_HANDLER 0x442000A1;PCDU_HANDLER 0x44250000;P60DOCK_HANDLER diff --git a/generators/bsp_hosted_returnvalues.csv b/generators/bsp_hosted_returnvalues.csv index fefbbb47..0f1de940 100644 --- a/generators/bsp_hosted_returnvalues.csv +++ b/generators/bsp_hosted_returnvalues.csv @@ -3,6 +3,16 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x0001;Failed;Unspecified system-wide code for failed.;1;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h 0x63a0;NVMB_KeyNotExists;Specified key does not exist in json file;160;NVM_PARAM_BASE;mission/memory/NVMParameterBase.h 0x6300;NVMB_Busy;No description;0;NVM_PARAM_BASE;mission/system/objects/Stack5VHandler.h +0x5100;IMTQ_InvalidCommandCode;No description;0;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h +0x5101;IMTQ_MgmMeasurementLowLevelError;No description;1;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h +0x5102;IMTQ_ActuateCmdLowLevelError;No description;2;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h +0x5103;IMTQ_ParameterMissing;No description;3;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h +0x5104;IMTQ_ParameterInvalid;No description;4;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h +0x5105;IMTQ_CcUnavailable;No description;5;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h +0x5106;IMTQ_InternalProcessingError;No description;6;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h +0x5107;IMTQ_RejectedWithoutReason;No description;7;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h +0x5108;IMTQ_CmdErrUnknown;No description;8;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h +0x51a7;IMTQ_UnexpectedSelfTestReply;The status reply to a self test command was received but no self test command has been sent. This should normally never happen.;167;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h 0x52b0;RWHA_SpiWriteFailure;No description;176;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h 0x52b1;RWHA_SpiReadFailure;Used by the spi send function to tell a failing read call;177;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h 0x52b2;RWHA_MissingStartSign;Can be used by the HDLC decoding mechanism to inform about a missing start sign 0x7E;178;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h @@ -13,19 +23,12 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x52b7;RWHA_SpiReadTimeout;Timeout when reading reply;183;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h 0x58a0;SUSS_ErrorUnlockMutex;No description;160;SUS_HANDLER;mission/devices/SusHandler.h 0x58a1;SUSS_ErrorLockMutex;No description;161;SUS_HANDLER;mission/devices/SusHandler.h -0x66a0;SADPL_CommandNotSupported;No description;160;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h -0x66a1;SADPL_DeploymentAlreadyExecuting;No description;161;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h -0x66a2;SADPL_MainSwitchTimeoutFailure;No description;162;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h -0x66a3;SADPL_SwitchingDeplSa1Failed;No description;163;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h -0x66a4;SADPL_SwitchingDeplSa2Failed;No description;164;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h -0x51a0;IMTQ_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;IMTQ_HANDLER;mission/devices/RwHandler.h -0x51a1;IMTQ_InvalidRampTime;Action Message with invalid ramp time was received.;161;IMTQ_HANDLER;mission/devices/RwHandler.h -0x51a2;IMTQ_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;IMTQ_HANDLER;mission/devices/RwHandler.h -0x51a3;IMTQ_ExecutionFailed;Command execution failed;163;IMTQ_HANDLER;mission/devices/RwHandler.h -0x51a4;IMTQ_CrcError;Reaction wheel reply has invalid crc;164;IMTQ_HANDLER;mission/devices/RwHandler.h -0x51a5;IMTQ_ValueNotRead;No description;165;IMTQ_HANDLER;mission/devices/RwHandler.h -0x51a6;IMTQ_CmdErrUnknown;No description;166;IMTQ_HANDLER;mission/devices/ImtqHandler.h -0x51a7;IMTQ_UnexpectedSelfTestReply;The status reply to a self test command was received but no self test command has been sent. This should normally never happen.;167;IMTQ_HANDLER;mission/devices/ImtqHandler.h +0x66a0;SADPL_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;SA_DEPL_HANDLER;mission/devices/RwHandler.h +0x66a1;SADPL_InvalidRampTime;Action Message with invalid ramp time was received.;161;SA_DEPL_HANDLER;mission/devices/RwHandler.h +0x66a2;SADPL_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;SA_DEPL_HANDLER;mission/devices/RwHandler.h +0x66a3;SADPL_ExecutionFailed;Command execution failed;163;SA_DEPL_HANDLER;mission/devices/RwHandler.h +0x66a4;SADPL_CrcError;Reaction wheel reply has invalid crc;164;SA_DEPL_HANDLER;mission/devices/RwHandler.h +0x66a5;SADPL_ValueNotRead;No description;165;SA_DEPL_HANDLER;mission/devices/RwHandler.h 0x50a0;SYRLINKS_CrcFailure;No description;160;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h 0x50a1;SYRLINKS_UartFraminOrParityErrorAck;No description;161;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h 0x50a2;SYRLINKS_BadCharacterAck;No description;162;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h diff --git a/generators/bsp_q7s_objects.csv b/generators/bsp_q7s_objects.csv index 56dbacef..91e3a505 100644 --- a/generators/bsp_q7s_objects.csv +++ b/generators/bsp_q7s_objects.csv @@ -29,6 +29,7 @@ 0x44120350;RW4 0x44130001;STAR_TRACKER 0x44130045;GPS_CONTROLLER +0x44140013;IMTQ_POLLING 0x44140014;IMTQ_HANDLER 0x442000A1;PCDU_HANDLER 0x44250000;P60DOCK_HANDLER diff --git a/generators/bsp_q7s_returnvalues.csv b/generators/bsp_q7s_returnvalues.csv index c03d3126..5b1dd409 100644 --- a/generators/bsp_q7s_returnvalues.csv +++ b/generators/bsp_q7s_returnvalues.csv @@ -3,6 +3,16 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x0001;Failed;Unspecified system-wide code for failed.;1;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h 0x63a0;NVMB_KeyNotExists;Specified key does not exist in json file;160;NVM_PARAM_BASE;mission/memory/NVMParameterBase.h 0x6300;NVMB_Busy;No description;0;NVM_PARAM_BASE;mission/system/objects/Stack5VHandler.h +0x5100;IMTQ_InvalidCommandCode;No description;0;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h +0x5101;IMTQ_MgmMeasurementLowLevelError;No description;1;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h +0x5102;IMTQ_ActuateCmdLowLevelError;No description;2;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h +0x5103;IMTQ_ParameterMissing;No description;3;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h +0x5104;IMTQ_ParameterInvalid;No description;4;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h +0x5105;IMTQ_CcUnavailable;No description;5;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h +0x5106;IMTQ_InternalProcessingError;No description;6;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h +0x5107;IMTQ_RejectedWithoutReason;No description;7;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h +0x5108;IMTQ_CmdErrUnknown;No description;8;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h +0x51a7;IMTQ_UnexpectedSelfTestReply;The status reply to a self test command was received but no self test command has been sent. This should normally never happen.;167;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h 0x52b0;RWHA_SpiWriteFailure;No description;176;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h 0x52b1;RWHA_SpiReadFailure;Used by the spi send function to tell a failing read call;177;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h 0x52b2;RWHA_MissingStartSign;Can be used by the HDLC decoding mechanism to inform about a missing start sign 0x7E;178;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h @@ -13,19 +23,12 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x52b7;RWHA_SpiReadTimeout;Timeout when reading reply;183;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h 0x58a0;SUSS_ErrorUnlockMutex;No description;160;SUS_HANDLER;mission/devices/SusHandler.h 0x58a1;SUSS_ErrorLockMutex;No description;161;SUS_HANDLER;mission/devices/SusHandler.h -0x66a0;SADPL_CommandNotSupported;No description;160;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h -0x66a1;SADPL_DeploymentAlreadyExecuting;No description;161;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h -0x66a2;SADPL_MainSwitchTimeoutFailure;No description;162;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h -0x66a3;SADPL_SwitchingDeplSa1Failed;No description;163;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h -0x66a4;SADPL_SwitchingDeplSa2Failed;No description;164;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h -0x51a0;IMTQ_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;IMTQ_HANDLER;mission/devices/RwHandler.h -0x51a1;IMTQ_InvalidRampTime;Action Message with invalid ramp time was received.;161;IMTQ_HANDLER;mission/devices/RwHandler.h -0x51a2;IMTQ_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;IMTQ_HANDLER;mission/devices/RwHandler.h -0x51a3;IMTQ_ExecutionFailed;Command execution failed;163;IMTQ_HANDLER;mission/devices/RwHandler.h -0x51a4;IMTQ_CrcError;Reaction wheel reply has invalid crc;164;IMTQ_HANDLER;mission/devices/RwHandler.h -0x51a5;IMTQ_ValueNotRead;No description;165;IMTQ_HANDLER;mission/devices/RwHandler.h -0x51a6;IMTQ_CmdErrUnknown;No description;166;IMTQ_HANDLER;mission/devices/ImtqHandler.h -0x51a7;IMTQ_UnexpectedSelfTestReply;The status reply to a self test command was received but no self test command has been sent. This should normally never happen.;167;IMTQ_HANDLER;mission/devices/ImtqHandler.h +0x66a0;SADPL_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;SA_DEPL_HANDLER;mission/devices/RwHandler.h +0x66a1;SADPL_InvalidRampTime;Action Message with invalid ramp time was received.;161;SA_DEPL_HANDLER;mission/devices/RwHandler.h +0x66a2;SADPL_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;SA_DEPL_HANDLER;mission/devices/RwHandler.h +0x66a3;SADPL_ExecutionFailed;Command execution failed;163;SA_DEPL_HANDLER;mission/devices/RwHandler.h +0x66a4;SADPL_CrcError;Reaction wheel reply has invalid crc;164;SA_DEPL_HANDLER;mission/devices/RwHandler.h +0x66a5;SADPL_ValueNotRead;No description;165;SA_DEPL_HANDLER;mission/devices/RwHandler.h 0x50a0;SYRLINKS_CrcFailure;No description;160;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h 0x50a1;SYRLINKS_UartFraminOrParityErrorAck;No description;161;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h 0x50a2;SYRLINKS_BadCharacterAck;No description;162;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h @@ -566,6 +569,7 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x53b6;STRH_StartrackerAlreadyBooted;Star tracker is already in firmware mode;182;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h 0x53b7;STRH_StartrackerRunningFirmware;Star tracker is in firmware mode but must be in bootloader mode to execute this command;183;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h 0x53b8;STRH_StartrackerRunningBootloader;Star tracker is in bootloader mode but must be in firmware mode to execute this command;184;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h +0x5300;STRH_NoReplyAvailable;No description;0;STR_HANDLER;linux/devices/ImtqPollingTask.h 0x5302;STRH_InvalidCrc;No description;2;STR_HANDLER;linux/devices/ScexHelper.h 0x5aa0;PTME_UnknownVcId;No description;160;PTME;linux/ipcore/Ptme.h 0x5fa0;PDEC_AbandonedCltuRetval;No description;160;PDEC_HANDLER;linux/ipcore/PdecHandler.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index 44dc754a..71f28a31 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** * @brief Auto-generated event translation file. Contains 257 translations. * @details - * Generated on: 2023-02-21 00:24:44 + * Generated on: 2023-02-21 10:44:59 */ #include "translateEvents.h" diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index dc27fca5..b8deca09 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -1,8 +1,8 @@ /** * @brief Auto-generated object translation file. * @details - * Contains 152 translations. - * Generated on: 2023-02-21 00:24:44 + * Contains 153 translations. + * Generated on: 2023-02-21 10:44:59 */ #include "translateObjects.h" @@ -37,6 +37,7 @@ const char *GYRO_3_L3G_HANDLER_STRING = "GYRO_3_L3G_HANDLER"; const char *RW4_STRING = "RW4"; const char *STAR_TRACKER_STRING = "STAR_TRACKER"; const char *GPS_CONTROLLER_STRING = "GPS_CONTROLLER"; +const char *IMTQ_POLLING_STRING = "IMTQ_POLLING"; const char *IMTQ_HANDLER_STRING = "IMTQ_HANDLER"; const char *PCDU_HANDLER_STRING = "PCDU_HANDLER"; const char *P60DOCK_HANDLER_STRING = "P60DOCK_HANDLER"; @@ -223,6 +224,8 @@ const char *translateObject(object_id_t object) { return STAR_TRACKER_STRING; case 0x44130045: return GPS_CONTROLLER_STRING; + case 0x44140013: + return IMTQ_POLLING_STRING; case 0x44140014: return IMTQ_HANDLER_STRING; case 0x442000A1: diff --git a/generators/requirements.txt b/generators/requirements.txt index cbdc29d2..ebe8f639 100644 --- a/generators/requirements.txt +++ b/generators/requirements.txt @@ -1,2 +1,2 @@ colorlog==6.7.0 -git+https://egit.irs.uni-stuttgart.de/fsfw/fsfwgen@66e31885a7c87fbb4340cd2a51a13e1196f377af#egg=fsfwgen +git+https://egit.irs.uni-stuttgart.de/fsfw/fsfwgen@v0.3.0#egg=fsfwgen diff --git a/linux/devices/CMakeLists.txt b/linux/devices/CMakeLists.txt index 7251b802..1b08ad84 100644 --- a/linux/devices/CMakeLists.txt +++ b/linux/devices/CMakeLists.txt @@ -3,8 +3,9 @@ if(EIVE_BUILD_GPSD_GPS_HANDLER) endif() target_sources( - ${OBSW_NAME} PRIVATE Max31865RtdLowlevelHandler.cpp ScexUartReader.cpp - ScexDleParser.cpp ScexHelper.cpp RwPollingTask.cpp) + ${OBSW_NAME} + PRIVATE Max31865RtdLowlevelHandler.cpp ScexUartReader.cpp ScexDleParser.cpp + ScexHelper.cpp RwPollingTask.cpp ImtqPollingTask.cpp) add_subdirectory(ploc) diff --git a/linux/devices/ImtqPollingTask.cpp b/linux/devices/ImtqPollingTask.cpp new file mode 100644 index 00000000..4d8790ee --- /dev/null +++ b/linux/devices/ImtqPollingTask.cpp @@ -0,0 +1,433 @@ +#include "ImtqPollingTask.h" + +#include +#include +#include +#include +#include +#include +#include + +#include "fsfw/FSFW.h" + +ImtqPollingTask::ImtqPollingTask(object_id_t imtqPollingTask) : SystemObject(imtqPollingTask) { + semaphore = SemaphoreFactory::instance()->createBinarySemaphore(); + semaphore->acquire(); + ipcLock = MutexFactory::instance()->createMutex(); + bufLock = MutexFactory::instance()->createMutex(); +} + +ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) { + while (true) { + ipcLock->lockMutex(); + state = InternalState::IDLE; + ipcLock->unlockMutex(); + semaphore->acquire(); + + comStatus = returnvalue::OK; + // Stopwatch watch; + switch (currentRequest) { + case imtq::RequestType::MEASURE_NO_ACTUATION: { + handleMeasureStep(); + break; + } + case imtq::RequestType::ACTUATE: { + handleActuateStep(); + break; + } + }; + } + return returnvalue::OK; +} + +void ImtqPollingTask::handleMeasureStep() { + size_t replyLen = 0; + uint8_t* replyPtr; + ImtqRepliesDefault replies(replyBuf.data()); + // Can be used later to verify correct timing (e.g. all data has been read) + clearReadFlagsDefault(replies); + auto i2cCmdExecMeasure = [&](imtq::CC::CC cc) { + ccToReplyPtrMeasure(replies, cc, &replyPtr, replyLen); + return i2cCmdExecDefault(cc, replyPtr, replyLen, imtq::MGM_MEASUREMENT_LOW_LEVEL_ERROR); + }; + + cmdLen = 1; + cmdBuf[0] = imtq::CC::GET_SYSTEM_STATE; + if (i2cCmdExecMeasure(imtq::CC::GET_SYSTEM_STATE) != returnvalue::OK) { + return; + } + + ignoreNextActuateRequest = + (replies.getSystemState()[2] == static_cast(imtq::mode::SELF_TEST)); + if (ignoreNextActuateRequest) { + // Do not command anything until self-test is done. + return; + } + + if (specialRequest != imtq::SpecialRequest::NONE) { + auto executeSelfTest = [&](imtq::selfTest::Axis axis) { + cmdBuf[0] = imtq::CC::SELF_TEST_CMD; + cmdBuf[1] = axis; + return i2cCmdExecMeasure(imtq::CC::SELF_TEST_CMD); + }; + // If a self-test is already ongoing, ignore the request. + if (replies.getSystemState()[2] != static_cast(imtq::mode::SELF_TEST)) { + switch (specialRequest) { + case (imtq::SpecialRequest::DO_SELF_TEST_POS_X): { + executeSelfTest(imtq::selfTest::Axis::X_POSITIVE); + break; + } + case (imtq::SpecialRequest::DO_SELF_TEST_NEG_X): { + executeSelfTest(imtq::selfTest::Axis::X_NEGATIVE); + break; + } + case (imtq::SpecialRequest::DO_SELF_TEST_POS_Y): { + executeSelfTest(imtq::selfTest::Axis::Y_POSITIVE); + break; + } + case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Y): { + executeSelfTest(imtq::selfTest::Axis::Y_NEGATIVE); + break; + } + case (imtq::SpecialRequest::DO_SELF_TEST_POS_Z): { + executeSelfTest(imtq::selfTest::Axis::Z_POSITIVE); + break; + } + case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Z): { + executeSelfTest(imtq::selfTest::Axis::Z_NEGATIVE); + break; + } + case (imtq::SpecialRequest::GET_SELF_TEST_RESULT): { + cmdBuf[0] = imtq::CC::GET_SELF_TEST_RESULT; + i2cCmdExecMeasure(imtq::CC::GET_SELF_TEST_RESULT); + break; + } + default: { + // Should never happen + break; + } + } + // We are done. Only request self test or results here. + return; + } + } + + cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT; + if (i2cCmdExecMeasure(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) { + return; + } + // Takes a bit of time to take measurements. Subtract a bit because of the delay of previous + // commands. + TaskFactory::delayTask(currentIntegrationTimeMs); + + cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT; + if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) { + return; + } + + cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA; + if (i2cCmdExecMeasure(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) { + return; + } + + cmdBuf[0] = imtq::CC::GET_CAL_MTM_MEASUREMENT; + if (i2cCmdExecMeasure(imtq::CC::GET_CAL_MTM_MEASUREMENT) != returnvalue::OK) { + return; + } + // sif::debug << "measure done" << std::endl; + return; +} + +void ImtqPollingTask::handleActuateStep() { + uint8_t* replyPtr = nullptr; + size_t replyLen = 0; + // No point when self-test mode is active. + if (ignoreNextActuateRequest) { + return; + } + ImtqRepliesWithTorque replies(replyBufActuation.data()); + // Can be used later to verify correct timing (e.g. all data has been read) + clearReadFlagsWithTorque(replies); + auto i2cCmdExecActuate = [&](imtq::CC::CC cc) { + ccToReplyPtrActuate(replies, cc, &replyPtr, replyLen); + return i2cCmdExecDefault(cc, replyPtr, replyLen, imtq::ACTUATE_CMD_LOW_LEVEL_ERROR); + }; + buildDipoleCommand(); + if (i2cCmdExecActuate(imtq::CC::START_ACTUATION_DIPOLE) != returnvalue::OK) { + return; + } + + cmdLen = 1; + cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT; + if (i2cCmdExecActuate(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) { + return; + } + + TaskFactory::delayTask(currentIntegrationTimeMs); + + cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT; + if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) { + return; + } + cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA; + if (i2cCmdExecActuate(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) { + return; + } + // sif::debug << "measure with torque done" << std::endl; + return; +} + +ReturnValue_t ImtqPollingTask::initialize() { return returnvalue::OK; } + +ReturnValue_t ImtqPollingTask::initializeInterface(CookieIF* cookie) { + i2cCookie = dynamic_cast(cookie); + if (i2cCookie == nullptr) { + sif::error << "ImtqPollingTask::initializeInterface: Invalid I2C cookie" << std::endl; + return returnvalue::FAILED; + } + i2cDev = i2cCookie->getDeviceFile().c_str(); + i2cAddr = i2cCookie->getAddress(); + return returnvalue::OK; +} + +ReturnValue_t ImtqPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData, + size_t sendLen) { + ImtqRequest request(sendData, sendLen); + { + MutexGuard mg(ipcLock); + currentRequest = request.getRequestType(); + if (currentRequest == imtq::RequestType::ACTUATE) { + std::memcpy(dipoles, request.getDipoles(), 6); + torqueDuration = request.getTorqueDuration(); + } + specialRequest = request.getSpecialRequest(); + if (state != InternalState::IDLE) { + return returnvalue::FAILED; + } + state = InternalState::BUSY; + } + semaphore->release(); + + return returnvalue::OK; +} + +ReturnValue_t ImtqPollingTask::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; } + +ReturnValue_t ImtqPollingTask::requestReceiveMessage(CookieIF* cookie, size_t requestLen) { + return returnvalue::OK; +} + +void ImtqPollingTask::ccToReplyPtrMeasure(ImtqRepliesDefault& replies, imtq::CC::CC cc, + uint8_t** replyBuf, size_t& replyLen) { + replyLen = imtq::getReplySize(cc); + switch (cc) { + case (imtq::CC::CC::GET_ENG_HK_DATA): { + *replyBuf = replies.engHk; + break; + } + case (imtq::CC::CC::SOFTWARE_RESET): { + *replyBuf = replies.swReset; + break; + } + case (imtq::CC::CC::GET_SYSTEM_STATE): { + *replyBuf = replies.systemState; + break; + } + case (imtq::CC::CC::START_MTM_MEASUREMENT): { + *replyBuf = replies.startMtmMeasurement; + break; + } + case (imtq::CC::CC::GET_RAW_MTM_MEASUREMENT): { + *replyBuf = replies.rawMgmMeasurement; + break; + } + case (imtq::CC::CC::GET_CAL_MTM_MEASUREMENT): { + *replyBuf = replies.calibMgmMeasurement; + break; + } + default: { + *replyBuf = replies.specialRequestReply; + break; + } + } +} + +void ImtqPollingTask::ccToReplyPtrActuate(ImtqRepliesWithTorque& replies, imtq::CC::CC cc, + uint8_t** replyBuf, size_t& replyLen) { + replyLen = imtq::getReplySize(cc); + switch (cc) { + case (imtq::CC::CC::START_ACTUATION_DIPOLE): { + *replyBuf = replies.dipoleActuation; + break; + } + case (imtq::CC::CC::GET_ENG_HK_DATA): { + *replyBuf = replies.engHk; + break; + } + case (imtq::CC::CC::START_MTM_MEASUREMENT): { + *replyBuf = replies.startMtmMeasurement; + break; + } + case (imtq::CC::CC::GET_RAW_MTM_MEASUREMENT): { + *replyBuf = replies.rawMgmMeasurement; + break; + } + default: { + *replyBuf = nullptr; + replyLen = 0; + break; + } + } +} +size_t ImtqPollingTask::getExchangeBufLen(imtq::SpecialRequest specialRequest) { + size_t baseLen = ImtqRepliesDefault::BASE_LEN; + switch (specialRequest) { + case (imtq::SpecialRequest::NONE): + case (imtq::SpecialRequest::DO_SELF_TEST_POS_X): + case (imtq::SpecialRequest::DO_SELF_TEST_NEG_X): + case (imtq::SpecialRequest::DO_SELF_TEST_POS_Y): + case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Y): + case (imtq::SpecialRequest::DO_SELF_TEST_POS_Z): + case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Z): { + break; + } + case (imtq::SpecialRequest::GET_SELF_TEST_RESULT): { + baseLen += imtq::replySize::SELF_TEST_RESULTS; + break; + } + } + return baseLen; +} + +void ImtqPollingTask::buildDipoleCommand() { + cmdBuf[0] = imtq::CC::CC::START_ACTUATION_DIPOLE; + uint8_t* serPtr = cmdBuf.data() + 1; + size_t serLen = 0; + for (uint8_t idx = 0; idx < 3; idx++) { + SerializeAdapter::serialize(&dipoles[idx], &serPtr, &serLen, cmdBuf.size(), + SerializeIF::Endianness::LITTLE); + } + SerializeAdapter::serialize(&torqueDuration, &serPtr, &serLen, cmdBuf.size(), + SerializeIF::Endianness::LITTLE); + cmdLen = 1 + serLen; +} + +ReturnValue_t ImtqPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, + size_t* size) { + imtq::RequestType currentRequest; + { + MutexGuard mg(ipcLock); + currentRequest = this->currentRequest; + } + + size_t replyLen = 0; + MutexGuard mg(bufLock); + if (currentRequest == imtq::RequestType::MEASURE_NO_ACTUATION) { + replyLen = getExchangeBufLen(specialRequest); + memcpy(exchangeBuf.data(), replyBuf.data(), replyLen); + } else { + replyLen = ImtqRepliesWithTorque::BASE_LEN; + memcpy(exchangeBuf.data(), replyBufActuation.data(), replyLen); + } + *buffer = exchangeBuf.data(); + *size = replyLen; + return comStatus; +} + +void ImtqPollingTask::clearReadFlagsDefault(ImtqRepliesDefault& replies) { + replies.calibMgmMeasurement[0] = false; + replies.rawMgmMeasurement[0] = false; + replies.systemState[0] = false; + replies.specialRequestReply[0] = false; + replies.engHk[0] = false; +} + +ReturnValue_t ImtqPollingTask::i2cCmdExecDefault(imtq::CC::CC cc, uint8_t* replyPtr, + size_t replyLen, ReturnValue_t comErrIfFails) { + ReturnValue_t res = performI2cFullRequest(replyPtr + 1, replyLen); + if (res != returnvalue::OK) { + sif::error << "IMTQ: I2C transaction for command 0x" << std::hex << std::setw(2) << cc + << " failed" << std::dec << std::endl; + comStatus = comErrIfFails; + return returnvalue::FAILED; + } + if (replyPtr[1] != cc) { + sif::warning << "IMTQ: Unexpected CC 0x" << std::hex << std::setw(2) + << static_cast(replyPtr[1]) << " for command 0x" << cc << std::dec + << std::endl; + comStatus = comErrIfFails; + return returnvalue::FAILED; + } + replyPtr[0] = true; + return returnvalue::OK; +} + +void ImtqPollingTask::clearReadFlagsWithTorque(ImtqRepliesWithTorque& replies) { + replies.dipoleActuation[0] = false; + replies.engHk[0] = false; + replies.rawMgmMeasurement[0] = false; + replies.startMtmMeasurement[0] = false; +} + +ReturnValue_t ImtqPollingTask::performI2cFullRequest(uint8_t* reply, size_t replyLen) { + int fd = 0; + if (cmdLen == 0 or reply == nullptr) { + return returnvalue::FAILED; + } + + { + UnixFileGuard fileHelper(i2cDev, fd, O_RDWR, "ImtqPollingTask::performI2cFullRequest"); + if (fileHelper.getOpenResult() != returnvalue::OK) { + return fileHelper.getOpenResult(); + } + if (ioctl(fd, I2C_SLAVE, i2cAddr) < 0) { + sif::warning << "Opening IMTQ slave device failed with code " << errno << ": " + << strerror(errno) << std::endl; + } + + int written = write(fd, cmdBuf.data(), cmdLen); + if (written < 0) { + sif::error << "IMTQ: Failed to send with error code " << errno + << ". Error description: " << strerror(errno) << std::endl; + return returnvalue::FAILED; + } else if (static_cast(written) != cmdLen) { + sif::error << "IMTQ: Could not write all bytes" << std::endl; + return returnvalue::FAILED; + } + } +#if FSFW_HAL_I2C_WIRETAPPING == 1 + sif::info << "Sent I2C data to bus " << deviceFile << ":" << std::endl; + arrayprinter::print(sendData, sendLen); +#endif + + // wait 1 ms like specified in the datasheet. This is the time the IMTQ needs + // to prepare a reply. + usleep(1000); + + { + UnixFileGuard fileHelper(i2cDev, fd, O_RDWR, "ImtqPollingTask::performI2cFullRequest"); + if (fileHelper.getOpenResult() != returnvalue::OK) { + return fileHelper.getOpenResult(); + } + if (ioctl(fd, I2C_SLAVE, i2cAddr) < 0) { + sif::warning << "Opening IMTQ slave device failed with code " << errno << ": " + << strerror(errno) << std::endl; + } + MutexGuard mg(bufLock); + int readLen = read(fd, reply, replyLen); + if (readLen != static_cast(replyLen)) { + if (readLen < 0) { + sif::warning << "IMTQ: Reading failed with error code " << errno << " | " << strerror(errno) + << std::endl; + } else { + sif::warning << "IMTQ: Read only" << readLen << " from " << replyLen << " bytes" + << std::endl; + } + } + } + if (reply[0] == 0xff or reply[1] == 0xff) { + sif::warning << "IMTQ: No reply available after 1 millisecond"; + return NO_REPLY_AVAILABLE; + } + return returnvalue::OK; +} diff --git a/linux/devices/ImtqPollingTask.h b/linux/devices/ImtqPollingTask.h new file mode 100644 index 00000000..00a05fc6 --- /dev/null +++ b/linux/devices/ImtqPollingTask.h @@ -0,0 +1,70 @@ +#ifndef LINUX_DEVICES_IMTQPOLLINGTASK_H_ +#define LINUX_DEVICES_IMTQPOLLINGTASK_H_ + +#include +#include + +#include "fsfw/devicehandlers/DeviceCommunicationIF.h" +#include "fsfw/objectmanager/SystemObject.h" +#include "fsfw/tasks/ExecutableObjectIF.h" +#include "mission/devices/devicedefinitions/imtqHelpers.h" + +class ImtqPollingTask : public SystemObject, + public ExecutableObjectIF, + public DeviceCommunicationIF { + public: + ImtqPollingTask(object_id_t imtqPollingTask); + + ReturnValue_t performOperation(uint8_t operationCode) override; + ReturnValue_t initialize() override; + + private: + static constexpr ReturnValue_t NO_REPLY_AVAILABLE = returnvalue::makeCode(2, 0); + + enum class InternalState { IDLE, BUSY } state = InternalState::IDLE; + imtq::RequestType currentRequest = imtq::RequestType::MEASURE_NO_ACTUATION; + + SemaphoreIF* semaphore; + ReturnValue_t comStatus = returnvalue::OK; + MutexIF* ipcLock; + MutexIF* bufLock; + I2cCookie* i2cCookie = nullptr; + const char* i2cDev = nullptr; + address_t i2cAddr = 0; + uint32_t currentIntegrationTimeMs = 10; + bool ignoreNextActuateRequest = false; + + imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE; + int16_t dipoles[3] = {}; + uint16_t torqueDuration = 0; + // uint8_t startActuateRawBuf[3] = {}; + + std::array cmdBuf; + std::array replyBuf; + std::array replyBufActuation; + std::array exchangeBuf; + size_t cmdLen = 0; + + // DeviceCommunicationIF overrides + ReturnValue_t initializeInterface(CookieIF* cookie) override; + ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override; + ReturnValue_t getSendSuccess(CookieIF* cookie) override; + ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override; + ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override; + + void ccToReplyPtrMeasure(ImtqRepliesDefault& replies, imtq::CC::CC cc, uint8_t** replyBuf, + size_t& replyLen); + void ccToReplyPtrActuate(ImtqRepliesWithTorque& replies, imtq::CC::CC cc, uint8_t** replyBuf, + size_t& replyLen); + void clearReadFlagsDefault(ImtqRepliesDefault& replies); + void clearReadFlagsWithTorque(ImtqRepliesWithTorque& replies); + size_t getExchangeBufLen(imtq::SpecialRequest specialRequest); + void buildDipoleCommand(); + void handleMeasureStep(); + void handleActuateStep(); + ReturnValue_t i2cCmdExecDefault(imtq::CC::CC cc, uint8_t* replyPtr, size_t replyLen, + ReturnValue_t comErrIfFails); + ReturnValue_t performI2cFullRequest(uint8_t* reply, size_t replyLen); +}; + +#endif /* LINUX_DEVICES_IMTQPOLLINGTASK_H_ */ diff --git a/linux/devices/RwPollingTask.cpp b/linux/devices/RwPollingTask.cpp index 1c209ed3..2d05ac8b 100644 --- a/linux/devices/RwPollingTask.cpp +++ b/linux/devices/RwPollingTask.cpp @@ -162,11 +162,15 @@ ReturnValue_t RwPollingTask::requestReceiveMessage(CookieIF* cookie, size_t requ ReturnValue_t RwPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) { RwCookie* rwCookie = dynamic_cast(cookie); - { - MutexGuard mg(ipcLock); - *buffer = rwCookie->replyBuf.data(); - *size = rwCookie->replyBuf.size(); + if (rwCookie == nullptr or rwCookie->bufLock == nullptr) { + return returnvalue::FAILED; } + { + MutexGuard mg(rwCookie->bufLock); + memcpy(rwCookie->exchangeBuf.data(), rwCookie->replyBuf.data(), rwCookie->replyBuf.size()); + } + *buffer = rwCookie->exchangeBuf.data(); + *size = rwCookie->exchangeBuf.size(); return returnvalue::OK; } @@ -248,6 +252,7 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf #endif size_t decodedFrameLen = 0; + MutexGuard mg(rwCookie.bufLock); while (decodedFrameLen < maxReplyLen) { // First byte already read in @@ -428,7 +433,12 @@ void RwPollingTask::handleSpecialRequests() { } uint8_t* replyBuf; size_t maxReadLen = idAndIdxToReadBuffer(specialRequestIds[idx], idx, &replyBuf); - readNextReply(*rwCookies[idx], replyBuf, maxReadLen); + result = readNextReply(*rwCookies[idx], replyBuf, maxReadLen); + if (result == returnvalue::OK) { + // The first byte is always a flag which shows whether the value was read + // properly at least once. + replyBuf[0] = true; + } } } diff --git a/linux/devices/RwPollingTask.h b/linux/devices/RwPollingTask.h index ae4bbeb1..8a3cc9e4 100644 --- a/linux/devices/RwPollingTask.h +++ b/linux/devices/RwPollingTask.h @@ -18,10 +18,14 @@ class RwCookie : public SpiCookie { static constexpr size_t REPLY_BUF_LEN = 524; RwCookie(uint8_t rwIdx, address_t spiAddress, gpioId_t chipSelect, const size_t maxSize, spi::SpiModes spiMode, uint32_t spiSpeed) - : SpiCookie(spiAddress, chipSelect, maxSize, spiMode, spiSpeed), rwIdx(rwIdx) {} + : SpiCookie(spiAddress, chipSelect, maxSize, spiMode, spiSpeed), rwIdx(rwIdx) { + bufLock = MutexFactory::instance()->createMutex(); + } private: std::array replyBuf{}; + std::array exchangeBuf{}; + MutexIF* bufLock; bool setSpeed = true; int32_t currentRwSpeed = 0; uint16_t currentRampTime = 0; @@ -56,6 +60,13 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev static constexpr uint32_t TIMEOUT_MS = 20; static constexpr uint8_t MAX_RETRIES_REPLY = 5; + // DeviceCommunicationIF overrides + ReturnValue_t initializeInterface(CookieIF* cookie) override; + ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override; + ReturnValue_t getSendSuccess(CookieIF* cookie) override; + ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override; + ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override; + ReturnValue_t writeAndReadAllRws(DeviceCommandId_t id); ReturnValue_t writeOneRwCmd(uint8_t rwIdx, int fd); ReturnValue_t readAllRws(DeviceCommandId_t id); @@ -64,15 +75,6 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev ReturnValue_t readNextReply(RwCookie& rwCookie, uint8_t* replyBuf, size_t maxReplyLen); void handleSpecialRequests(); - ReturnValue_t initializeInterface(CookieIF* cookie) override; - - ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override; - - ReturnValue_t getSendSuccess(CookieIF* cookie) override; - - ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override; - - ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override; ReturnValue_t openSpi(int flags, int& fd); ReturnValue_t pullCsLow(gpioId_t gpioId, GpioIF& gpioIF); void prepareSimpleCommand(DeviceCommandId_t id); diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index 44dc754a..71f28a31 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** * @brief Auto-generated event translation file. Contains 257 translations. * @details - * Generated on: 2023-02-21 00:24:44 + * Generated on: 2023-02-21 10:44:59 */ #include "translateEvents.h" diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index dc27fca5..b8deca09 100644 --- a/linux/fsfwconfig/objects/translateObjects.cpp +++ b/linux/fsfwconfig/objects/translateObjects.cpp @@ -1,8 +1,8 @@ /** * @brief Auto-generated object translation file. * @details - * Contains 152 translations. - * Generated on: 2023-02-21 00:24:44 + * Contains 153 translations. + * Generated on: 2023-02-21 10:44:59 */ #include "translateObjects.h" @@ -37,6 +37,7 @@ const char *GYRO_3_L3G_HANDLER_STRING = "GYRO_3_L3G_HANDLER"; const char *RW4_STRING = "RW4"; const char *STAR_TRACKER_STRING = "STAR_TRACKER"; const char *GPS_CONTROLLER_STRING = "GPS_CONTROLLER"; +const char *IMTQ_POLLING_STRING = "IMTQ_POLLING"; const char *IMTQ_HANDLER_STRING = "IMTQ_HANDLER"; const char *PCDU_HANDLER_STRING = "PCDU_HANDLER"; const char *P60DOCK_HANDLER_STRING = "P60DOCK_HANDLER"; @@ -223,6 +224,8 @@ const char *translateObject(object_id_t object) { return STAR_TRACKER_STRING; case 0x44130045: return GPS_CONTROLLER_STRING; + case 0x44140013: + return IMTQ_POLLING_STRING; case 0x44140014: return IMTQ_HANDLER_STRING; case 0x442000A1: diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index fc30f053..1f3e65de 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include "OBSWConfig.h" #include "eive/definitions.h" @@ -60,8 +61,8 @@ ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) { DeviceHandlerIF::PERFORM_OPERATION); thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE); thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.2, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.25, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.25, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.3, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.3, DeviceHandlerIF::GET_READ); #endif // These are actually part of another bus, but this works, so keep it like this for now #if OBSW_ADD_TMP_DEVICES == 1 @@ -585,98 +586,76 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg if (cfg.scheduleImtq) { // This is the MTM measurement cycle thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); + imtq::ComStep::DHB_OP); thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::SEND_WRITE); + imtq::ComStep::START_MEASURE_SEND); thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, - DeviceHandlerIF::GET_READ); + imtq::ComStep::START_MEASURE_GET); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, + imtq::ComStep::READ_MEASURE_SEND); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, + imtq::ComStep::READ_MEASURE_GET); } - thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::acs::SCHED_BLOCK_3_PERIOD, 0); + thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::acs::SCHED_BLOCK_4_PERIOD, 0); if (cfg.scheduleImtq) { // This is the torquing cycle. - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD, - DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_5_PERIOD, + imtq::ComStep::START_ACTUATE_SEND); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_5_PERIOD, + imtq::ComStep::START_ACTUATE_GET); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_6_PERIOD, + imtq::ComStep::READ_ACTUATE_SEND); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_6_PERIOD, + imtq::ComStep::READ_ACTUATE_GET); } if (cfg.scheduleRws) { // this is the torquing cycle - thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD, + thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_5_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD, + thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_5_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD, + thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_5_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD, + thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_5_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD, - DeviceHandlerIF::SEND_WRITE); - - thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_5_PERIOD, - DeviceHandlerIF::SEND_READ); + DeviceHandlerIF::SEND_WRITE); thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_5_PERIOD, - DeviceHandlerIF::SEND_READ); + DeviceHandlerIF::SEND_WRITE); thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_5_PERIOD, - DeviceHandlerIF::SEND_READ); + DeviceHandlerIF::SEND_WRITE); thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_5_PERIOD, - DeviceHandlerIF::SEND_READ); + DeviceHandlerIF::SEND_WRITE); thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_5_PERIOD, - DeviceHandlerIF::GET_READ); + DeviceHandlerIF::GET_WRITE); thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_5_PERIOD, - DeviceHandlerIF::GET_READ); + DeviceHandlerIF::GET_WRITE); thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_5_PERIOD, - DeviceHandlerIF::GET_READ); + DeviceHandlerIF::GET_WRITE); thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_5_PERIOD, + DeviceHandlerIF::GET_WRITE); + + thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_7_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_7_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_7_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_7_PERIOD, + DeviceHandlerIF::SEND_READ); + + thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_7_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_7_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_7_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_7_PERIOD, DeviceHandlerIF::GET_READ); } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index e519d2e8..0b7f5c18 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -5,6 +5,7 @@ #include #include #include +#include #include #include "acs/ActuatorCmd.h" @@ -19,7 +20,6 @@ #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" #include "mission/devices/devicedefinitions/SusDefinitions.h" -#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h" #include "mission/trace.h" class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF { @@ -84,7 +84,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes ACS::SensorValues sensorValues; /* ACS Actuation Datasets */ - IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER); + imtq::DipoleActuationSet dipoleSet = imtq::DipoleActuationSet(objects::IMTQ_HANDLER); rws::RwSpeedActuationSet rw1SpeedSet = rws::RwSpeedActuationSet(objects::RW1); rws::RwSpeedActuationSet rw2SpeedSet = rws::RwSpeedActuationSet(objects::RW2); rws::RwSpeedActuationSet rw3SpeedSet = rws::RwSpeedActuationSet(objects::RW3); diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index f4e46c69..7dcbddc7 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -10,7 +10,7 @@ #include #include #include -#include +#include #include #include #include @@ -785,7 +785,7 @@ void ThermalController::copyDevices() { } { - lp_var_t tempMgt = lp_var_t(objects::IMTQ_HANDLER, IMTQ::MCU_TEMPERATURE); + lp_var_t tempMgt = lp_var_t(objects::IMTQ_HANDLER, imtq::MCU_TEMPERATURE); PoolReadGuard pg(&tempMgt, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() != returnvalue::OK) { sif::warning << "ThermalController: Failed to read MGT temperature" << std::endl; diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index 92d4c5ff..49e3d5fa 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -1,6 +1,7 @@ #ifndef SENSORVALUES_H_ #define SENSORVALUES_H_ +#include #include #include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h" @@ -10,7 +11,6 @@ #include "mission/devices/devicedefinitions/GPSDefinitions.h" #include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h" #include "mission/devices/devicedefinitions/SusDefinitions.h" -#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h" namespace ACS { @@ -35,7 +35,8 @@ class SensorValues { MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER); RM3100::Rm3100PrimaryDataset mgm3Rm3100Set = RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER); - IMTQ::RawMtmMeasurementSet imtqMgmSet = IMTQ::RawMtmMeasurementSet(objects::IMTQ_HANDLER); + imtq::RawMtmMeasurementNoTorque imtqMgmSet = + imtq::RawMtmMeasurementNoTorque(objects::IMTQ_HANDLER); std::array susSets{ SUS::SusDataset(objects::SUS_0_N_LOC_XFYFZM_PT_XF), diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index 93e9dae8..18c002c4 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -1,4 +1,3 @@ -#include #include #include #include @@ -7,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -32,10 +32,13 @@ static constexpr bool ACTUATION_WIRETAPPING = false; ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, power::Switch_t pwrSwitcher) : DeviceHandlerBase(objectId, comIF, comCookie), - engHkDataset(this), - calMtmMeasurementSet(this), - rawMtmMeasurementSet(this), + statusSet(this), dipoleSet(*this), + rawMtmNoTorque(this), + hkDatasetNoTorque(this), + rawMtmWithTorque(this), + hkDatasetWithTorque(this), + calMtmMeasurementSet(this), posXselfTestDataset(this), negXselfTestDataset(this), posYselfTestDataset(this), @@ -48,9 +51,64 @@ ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comC } } +ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) { + uint8_t dhbOpCode = DeviceHandlerIF::PERFORM_OPERATION; + switch (static_cast(opCode)) { + case (imtq::ComStep::DHB_OP): { + break; + } + case (imtq::ComStep::START_MEASURE_SEND): { + requestStep = imtq::RequestType::MEASURE_NO_ACTUATION; + dhbOpCode = DeviceHandlerIF::SEND_WRITE; + break; + } + case (imtq::ComStep::START_MEASURE_GET): { + requestStep = imtq::RequestType::MEASURE_NO_ACTUATION; + dhbOpCode = DeviceHandlerIF::GET_WRITE; + break; + } + case (imtq::ComStep::READ_MEASURE_SEND): { + requestStep = imtq::RequestType::MEASURE_NO_ACTUATION; + dhbOpCode = DeviceHandlerIF::SEND_READ; + break; + } + case (imtq::ComStep::READ_MEASURE_GET): { + requestStep = imtq::RequestType::MEASURE_NO_ACTUATION; + dhbOpCode = DeviceHandlerIF::GET_READ; + break; + } + case (imtq::ComStep::START_ACTUATE_SEND): { + requestStep = imtq::RequestType::ACTUATE; + dhbOpCode = DeviceHandlerIF::SEND_WRITE; + break; + } + case (imtq::ComStep::START_ACTUATE_GET): { + requestStep = imtq::RequestType::ACTUATE; + dhbOpCode = DeviceHandlerIF::GET_WRITE; + break; + } + case (imtq::ComStep::READ_ACTUATE_SEND): { + requestStep = imtq::RequestType::ACTUATE; + dhbOpCode = DeviceHandlerIF::SEND_READ; + break; + } + case (imtq::ComStep::READ_ACTUATE_GET): { + requestStep = imtq::RequestType::ACTUATE; + dhbOpCode = DeviceHandlerIF::GET_READ; + break; + } + default: { + sif::error << "ImtqHandler: Unexpected COM step" << std::endl; + break; + } + } + return DeviceHandlerBase::performOperation(dhbOpCode); +} + ImtqHandler::~ImtqHandler() = default; void ImtqHandler::doStartUp() { + updatePeriodicReply(true, imtq::cmdIds::REPLY); if (goToNormalMode) { setMode(MODE_NORMAL); } else { @@ -58,64 +116,23 @@ void ImtqHandler::doStartUp() { } } -void ImtqHandler::doShutDown() { setMode(_MODE_POWER_DOWN); } +void ImtqHandler::doShutDown() { + updatePeriodicReply(false, imtq::cmdIds::REPLY); + specialRequestActive = false; + firstReplyCycle = true; + setMode(_MODE_POWER_DOWN); +} ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { - bool buildCommand = true; - // Depending on the normal polling mode configuration, 3-4 communication steps are recommended - switch (communicationStep) { - case CommunicationStep::GET_ENG_HK_DATA: - *id = IMTQ::GET_ENG_HK_DATA; - communicationStep = CommunicationStep::START_MTM_MEASUREMENT; - break; - case CommunicationStep::START_MTM_MEASUREMENT: - *id = IMTQ::START_MTM_MEASUREMENT; - if (pollingMode == NormalPollingMode::BOTH or - pollingMode == NormalPollingMode::UNCALIBRATED) { - communicationStep = CommunicationStep::GET_RAW_MTM_MEASUREMENT; - } else { - communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT; - } - break; - case CommunicationStep::GET_RAW_MTM_MEASUREMENT: - if (integrationTimeCd.getRemainingMillis() > 0) { - TaskFactory::delayTask(integrationTimeCd.getRemainingMillis()); - } - *id = IMTQ::GET_RAW_MTM_MEASUREMENT; - if (pollingMode == NormalPollingMode::BOTH) { - communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT; - } else { - communicationStep = CommunicationStep::DIPOLE_ACTUATION; - } - break; - case CommunicationStep::GET_CAL_MTM_MEASUREMENT: - if (integrationTimeCd.getRemainingMillis() > 0) { - TaskFactory::delayTask(integrationTimeCd.getRemainingMillis()); - } - *id = IMTQ::GET_CAL_MTM_MEASUREMENT; - communicationStep = CommunicationStep::DIPOLE_ACTUATION; - break; - case CommunicationStep::DIPOLE_ACTUATION: { - // If the dipole is not commanded but set by the ACS control algorithm, - // the dipoles will be set by the ACS controller directly using the dipole local pool set. - // This set has a flag to determine whether the ACS controller actually set any new input. - MutexGuard mg(torquer::lazyLock()); - if (torquer::NEW_ACTUATION_FLAG) { - *id = IMTQ::START_ACTUATION_DIPOLE; - torquer::NEW_ACTUATION_FLAG = false; - } else { - buildCommand = false; - } - communicationStep = CommunicationStep::GET_ENG_HK_DATA; - break; + switch (requestStep) { + case (imtq::RequestType::MEASURE_NO_ACTUATION): { + *id = imtq::cmdIds::REQUEST; + return buildCommandFromCommand(*id, nullptr, 0); + } + case (imtq::RequestType::ACTUATE): { + *id = imtq::cmdIds::START_ACTUATION_DIPOLE; + return buildCommandFromCommand(*id, nullptr, 0); } - default: - sif::debug << "IMTQHandler::buildNormalDeviceCommand: Invalid communication step" - << std::endl; - break; - } - if (buildCommand) { - return buildCommandFromCommand(*id, nullptr, 0); } return NOTHING_TO_SEND; } @@ -127,120 +144,81 @@ ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t* commandData, size_t commandDataLen) { + auto genericSpecialRequest = [&](imtq::SpecialRequest specialRequest) { + ImtqRequest request(commandBuffer, sizeof(commandBuffer)); + request.setMeasureRequest(specialRequest); + specialRequestActive = true; + rawPacket = commandBuffer; + rawPacketLen = ImtqRequest::REQUEST_LEN; + }; 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; + case (imtq::cmdIds::POS_X_SELF_TEST): { + genericSpecialRequest(imtq::SpecialRequest::DO_SELF_TEST_POS_X); return returnvalue::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; + case (imtq::cmdIds::NEG_X_SELF_TEST): { + genericSpecialRequest(imtq::SpecialRequest::DO_SELF_TEST_NEG_X); return returnvalue::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; + case (imtq::cmdIds::POS_Y_SELF_TEST): { + genericSpecialRequest(imtq::SpecialRequest::DO_SELF_TEST_POS_Y); return returnvalue::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; + case (imtq::cmdIds::NEG_Y_SELF_TEST): { + genericSpecialRequest(imtq::SpecialRequest::DO_SELF_TEST_NEG_Y); return returnvalue::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; + case (imtq::cmdIds::POS_Z_SELF_TEST): { + genericSpecialRequest(imtq::SpecialRequest::DO_SELF_TEST_POS_Z); return returnvalue::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; + case (imtq::cmdIds::NEG_Z_SELF_TEST): { + genericSpecialRequest(imtq::SpecialRequest::DO_SELF_TEST_NEG_Z); return returnvalue::OK; } - case (IMTQ::GET_SELF_TEST_RESULT): { - commandBuffer[0] = IMTQ::CC::GET_SELF_TEST_RESULT; - rawPacket = commandBuffer; - rawPacketLen = 1; + case (imtq::cmdIds::GET_SELF_TEST_RESULT): { + genericSpecialRequest(imtq::SpecialRequest::GET_SELF_TEST_RESULT); return returnvalue::OK; } - case (IMTQ::START_ACTUATION_DIPOLE): { + case (imtq::cmdIds::REQUEST): { + ImtqRequest request(commandBuffer, sizeof(commandBuffer)); + request.setMeasureRequest(imtq::SpecialRequest::NONE); + rawPacket = commandBuffer; + rawPacketLen = ImtqRequest::REQUEST_LEN; + return returnvalue::OK; + } + case (imtq::cmdIds::START_ACTUATION_DIPOLE): { /* IMTQ expects low byte first */ - commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE; + // commandBuffer[0] = imtq::CC::START_ACTUATION_DIPOLE; if (commandData != nullptr && commandDataLen < 8) { return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; } - ReturnValue_t result; - { + ImtqRequest request(commandBuffer, sizeof(commandBuffer)); + // Commands override anything which was set in the software + if (commandData != nullptr) { // Read set dipole values from local pool PoolReadGuard pg(&dipoleSet); - // Commands override anything which was set in the software - if (commandData != nullptr) { - dipoleSet.setValidityBufferGeneration(false); - result = dipoleSet.deSerialize(&commandData, &commandDataLen, - SerializeIF::Endianness::NETWORK); - dipoleSet.setValidityBufferGeneration(true); - if (result != returnvalue::OK) { - return result; - } - } + int16_t xDipole = 0, yDipole = 0, zDipole = 0; + uint16_t torqueDuration = 0; + dipoleSet.xDipole = xDipole; + dipoleSet.yDipole = yDipole; + dipoleSet.zDipole = zDipole; + dipoleSet.currentTorqueDurationMs = torqueDuration; } + + request.setActuateRequest(dipoleSet.xDipole.value, dipoleSet.yDipole.value, + dipoleSet.zDipole.value, dipoleSet.currentTorqueDurationMs.value); if (ACTUATION_WIRETAPPING) { sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.xDipole.value << ", y = " << dipoleSet.yDipole.value << ", z = " << dipoleSet.zDipole.value << ", duration = " << dipoleSet.currentTorqueDurationMs.value << std::endl; } - result = buildDipoleActuationCommand(); - if (result != returnvalue::OK) { - return result; - } MutexGuard mg(torquer::lazyLock()); torquer::TORQUEING = true; torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value); - return result; - } - case (IMTQ::GET_ENG_HK_DATA): { - commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA; rawPacket = commandBuffer; - rawPacketLen = 1; - return returnvalue::OK; - } - case (IMTQ::GET_COMMANDED_DIPOLE): { - commandBuffer[0] = IMTQ::CC::GET_COMMANDED_DIPOLE; - rawPacket = commandBuffer; - rawPacketLen = 1; - return returnvalue::OK; - } - case (IMTQ::START_MTM_MEASUREMENT): { - commandBuffer[0] = IMTQ::CC::START_MTM_MEASUREMENT; - integrationTimeCd.resetTimer(); - rawPacket = commandBuffer; - rawPacketLen = 1; - return returnvalue::OK; - } - case (IMTQ::GET_CAL_MTM_MEASUREMENT): { - commandBuffer[0] = IMTQ::CC::GET_CAL_MTM_MEASUREMENT; - rawPacket = commandBuffer; - rawPacketLen = 1; - return returnvalue::OK; - } - case (IMTQ::GET_RAW_MTM_MEASUREMENT): { - commandBuffer[0] = IMTQ::CC::GET_RAW_MTM_MEASUREMENT; - rawPacket = commandBuffer; - rawPacketLen = 1; + rawPacketLen = ImtqRequest::REQUEST_LEN; return returnvalue::OK; } default: @@ -249,148 +227,161 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma return returnvalue::FAILED; } -ReturnValue_t ImtqHandler::buildDipoleActuationCommand() { - commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE; - uint8_t* serPtr = commandBuffer + 1; - size_t serSize = 1; - dipoleSet.setValidityBufferGeneration(false); - ReturnValue_t result = dipoleSet.serialize(&serPtr, &serSize, sizeof(commandBuffer), - SerializeIF::Endianness::LITTLE); - dipoleSet.setValidityBufferGeneration(true); - if (result != returnvalue::OK) { - return result; - } - rawPacket = commandBuffer; - rawPacketLen = 9; - return result; -} - void ImtqHandler::fillCommandAndReplyMap() { - insertInCommandAndReplyMap(IMTQ::POS_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); - insertInCommandAndReplyMap(IMTQ::NEG_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); - insertInCommandAndReplyMap(IMTQ::POS_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); - insertInCommandAndReplyMap(IMTQ::NEG_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); - insertInCommandAndReplyMap(IMTQ::POS_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); - insertInCommandAndReplyMap(IMTQ::NEG_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); - insertInCommandAndReplyMap(IMTQ::GET_SELF_TEST_RESULT, 1, nullptr, IMTQ::SIZE_SELF_TEST_RESULTS); - insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); - insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset, IMTQ::SIZE_ENG_HK_DATA_REPLY); - insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr, - IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY); - insertInCommandAndReplyMap(IMTQ::START_MTM_MEASUREMENT, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); - insertInCommandAndReplyMap(IMTQ::GET_CAL_MTM_MEASUREMENT, 1, &calMtmMeasurementSet, - IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT); - insertInCommandAndReplyMap(IMTQ::GET_RAW_MTM_MEASUREMENT, 1, &rawMtmMeasurementSet, - IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT); + insertInCommandMap(imtq::cmdIds::REQUEST); + insertInCommandMap(imtq::cmdIds::START_ACTUATION_DIPOLE); + insertInReplyMap(imtq::cmdIds::REPLY, 5, nullptr, 0, true); + insertInCommandMap(imtq::cmdIds::POS_X_SELF_TEST); + insertInCommandMap(imtq::cmdIds::NEG_X_SELF_TEST); + insertInCommandMap(imtq::cmdIds::POS_Y_SELF_TEST); + insertInCommandMap(imtq::cmdIds::NEG_Y_SELF_TEST); + insertInCommandMap(imtq::cmdIds::POS_Z_SELF_TEST); + insertInCommandMap(imtq::cmdIds::NEG_Z_SELF_TEST); + insertInCommandMap(imtq::cmdIds::GET_SELF_TEST_RESULT); } ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) { - ReturnValue_t result = returnvalue::OK; - - switch (*start) { - case (IMTQ::CC::START_ACTUATION_DIPOLE): - *foundLen = IMTQ::SIZE_STATUS_REPLY; - *foundId = IMTQ::START_ACTUATION_DIPOLE; - break; - case (IMTQ::CC::START_MTM_MEASUREMENT): - *foundLen = IMTQ::SIZE_STATUS_REPLY; - *foundId = IMTQ::START_MTM_MEASUREMENT; - break; - case (IMTQ::CC::GET_ENG_HK_DATA): - *foundLen = IMTQ::SIZE_ENG_HK_DATA_REPLY; - *foundId = IMTQ::GET_ENG_HK_DATA; - break; - case (IMTQ::CC::GET_COMMANDED_DIPOLE): - *foundLen = IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY; - *foundId = IMTQ::GET_COMMANDED_DIPOLE; - break; - case (IMTQ::CC::GET_CAL_MTM_MEASUREMENT): - *foundLen = IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT; - *foundId = IMTQ::GET_CAL_MTM_MEASUREMENT; - break; - case (IMTQ::CC::GET_RAW_MTM_MEASUREMENT): - *foundLen = IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT; - *foundId = IMTQ::GET_RAW_MTM_MEASUREMENT; - break; - case (IMTQ::CC::SELF_TEST_CMD): - *foundLen = IMTQ::SIZE_STATUS_REPLY; - result = getSelfTestCommandId(foundId); - break; - case (IMTQ::CC::GET_SELF_TEST_RESULT): - *foundLen = IMTQ::SIZE_SELF_TEST_RESULTS; - *foundId = IMTQ::GET_SELF_TEST_RESULT; - break; - case (IMTQ::CC::PAST_AVAILABLE_RESPONSE_BYTES): { - sif::warning << "IMTQHandler::scanForReply: Read 0xFF command byte, reading past available " - "bytes. Keep 1 ms delay between I2C send and read" - << std::endl; - result = IGNORE_REPLY_DATA; - break; - } - default: - sif::debug << "IMTQHandler::scanForReply: Reply with length " << remainingSize - << "contains invalid command code " << static_cast(*start) << std::endl; - result = IGNORE_REPLY_DATA; - break; + if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) { + return IGNORE_FULL_PACKET; } - - return result; + if (remainingSize > 0) { + *foundLen = remainingSize; + *foundId = imtq::cmdIds::REPLY; + return returnvalue::OK; + } + return returnvalue::FAILED; } ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { - ReturnValue_t result = returnvalue::OK; - - result = parseStatusByte(packet); - - if (result != returnvalue::OK) { - return result; + ReturnValue_t result; + ReturnValue_t status = returnvalue::OK; + if(getMode() != MODE_NORMAL) { + // Ignore replies during transitions. + return returnvalue::OK; } + // arrayprinter::print(packet, ImtqReplies::BASE_LEN); + if (requestStep == imtq::RequestType::MEASURE_NO_ACTUATION) { + requestStep = imtq::RequestType::ACTUATE; + // sif::debug << "handle measure" << std::endl; + ImtqRepliesDefault replies(packet); + if (specialRequestActive) { + if (replies.wasSpecialRequestRead()) { + uint8_t* specialRequest = replies.getSpecialRequest(); + imtq::CC::CC cc = static_cast(specialRequest[0]); + result = parseStatusByte(cc, packet); + if (result != returnvalue::OK) { + status = result; + } + if (cc == imtq::CC::CC::GET_SELF_TEST_RESULT) { + handleSelfTestReply(specialRequest); + } + // For a special request, the other stuff was not read, so return here. + return status; + } else { + sif::warning << "IMTQ: Possible timing issue, special request was not read" << std::endl; + } + specialRequestActive = false; + } + if (not replies.wasEngHkRead() and not firstReplyCycle) { + sif::warning << "IMTQ: Possible timing issue, ENG HK was not read" << std::endl; + } + // Still read it, even if it is old. Better than nothing + uint8_t* engHkReply = replies.getEngHk(); + result = parseStatusByte(imtq::CC::GET_ENG_HK_DATA, engHkReply); + if (result == returnvalue::OK) { + fillEngHkDataset(hkDatasetNoTorque, engHkReply); + } else { + status = result; + } - switch (id) { - case (IMTQ::POS_X_SELF_TEST): - case (IMTQ::NEG_X_SELF_TEST): - case (IMTQ::POS_Y_SELF_TEST): - case (IMTQ::NEG_Y_SELF_TEST): - case (IMTQ::POS_Z_SELF_TEST): - case (IMTQ::NEG_Z_SELF_TEST): - case (IMTQ::START_ACTUATION_DIPOLE): - case (IMTQ::START_MTM_MEASUREMENT): - /* Replies only the status byte which is already handled with parseStatusByte */ - break; - case (IMTQ::GET_ENG_HK_DATA): - fillEngHkDataset(packet); - break; - case (IMTQ::GET_COMMANDED_DIPOLE): - handleGetCommandedDipoleReply(packet); - break; - case (IMTQ::GET_CAL_MTM_MEASUREMENT): - fillCalibratedMtmDataset(packet); - break; - case (IMTQ::GET_RAW_MTM_MEASUREMENT): - fillRawMtmDataset(packet); - break; - case (IMTQ::GET_SELF_TEST_RESULT): - handleSelfTestReply(packet); - break; - default: { - sif::debug << "IMTQHandler::interpretDeviceReply: Unknown device reply id" << std::endl; - return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; + if (not replies.wasGetSystemStateRead() and not firstReplyCycle) { + sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl; + } + uint8_t* sysStateReply = replies.getSystemState(); + result = parseStatusByte(imtq::CC::GET_SYSTEM_STATE, sysStateReply); + if (result == returnvalue::OK) { + fillSystemStateIntoDataset(sysStateReply); + } else { + status = result; + } + + if (not replies.wasGetRawMgmMeasurementRead() and not firstReplyCycle) { + sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl; + } + uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement(); + result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement); + if (result == returnvalue::OK) { + fillRawMtmDataset(rawMgmMeasurement); + } else { + status = result; + } + + if (not replies.wasCalibMgmMeasurementRead() and not firstReplyCycle) { + sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl; + } + uint8_t* calibMgmMeasurement = replies.getCalibMgmMeasurement(); + result = parseStatusByte(imtq::CC::GET_CAL_MTM_MEASUREMENT, calibMgmMeasurement); + if (result == returnvalue::OK) { + fillRawMtmDataset(calibMgmMeasurement); + } else { + status = result; + } + } else { + // sif::debug << "handle measure with torque" << std::endl; + requestStep = imtq::RequestType::MEASURE_NO_ACTUATION; + ImtqRepliesWithTorque replies(packet); + if (replies.wasDipoleActuationRead()) { + parseStatusByte(imtq::CC::START_ACTUATION_DIPOLE, replies.getDipoleActuation()); + } else if (not firstReplyCycle) { + sif::warning << "IMTQ: Possible timing issue, start actuation dipole status was not read" + << std::endl; + } + + if (not replies.wasGetRawMgmMeasurementRead() and not firstReplyCycle) { + sif::warning << "IMTQ: Possible timing issue, was MGM measurement with torque was not read" + << std::endl; + } + uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement(); + result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement); + if (result == returnvalue::OK) { + fillRawMtmDataset(rawMgmMeasurement); + } else { + status = result; + } + + if (not replies.wasEngHkRead() and not firstReplyCycle) { + sif::warning << "IMTQ: Possible timing issue, engineering HK with torque was not read" + << std::endl; + } + uint8_t* engHkReply = replies.getEngHk(); + result = parseStatusByte(imtq::CC::GET_ENG_HK_DATA, engHkReply); + if (result != returnvalue::OK) { + return result; + } else { + status = result; + } + fillEngHkDataset(hkDatasetNoTorque, engHkReply); + if (firstReplyCycle) { + firstReplyCycle = false; } } - - return returnvalue::OK; + return status; } -void ImtqHandler::setNormalDatapoolEntriesInvalid() {} - LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) { - if (sid == engHkDataset.getSid()) { - return &engHkDataset; + if (sid == hkDatasetNoTorque.getSid()) { + return &hkDatasetNoTorque; + } else if (sid == dipoleSet.getSid()) { + return &dipoleSet; + } else if (sid == hkDatasetWithTorque.getSid()) { + return &hkDatasetWithTorque; + } else if (sid == rawMtmWithTorque.getSid()) { + return &rawMtmWithTorque; } else if (sid == calMtmMeasurementSet.getSid()) { return &calMtmMeasurementSet; - } else if (sid == rawMtmMeasurementSet.getSid()) { - return &rawMtmMeasurementSet; + } else if (sid == rawMtmNoTorque.getSid()) { + return &rawMtmNoTorque; } else if (sid == posXselfTestDataset.getSid()) { return &posXselfTestDataset; } else if (sid == negXselfTestDataset.getSid()) { @@ -413,442 +404,419 @@ uint32_t ImtqHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { ret ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { - /** Entries of engineering housekeeping dataset */ - localDataPoolMap.emplace(IMTQ::DIGITAL_VOLTAGE_MV, new PoolEntry({0})); - localDataPoolMap.emplace(IMTQ::ANALOG_VOLTAGE_MV, new PoolEntry({0})); - localDataPoolMap.emplace(IMTQ::DIGITAL_CURRENT, new PoolEntry({0})); - localDataPoolMap.emplace(IMTQ::ANALOG_CURRENT, new PoolEntry({0})); - localDataPoolMap.emplace(IMTQ::COIL_X_CURRENT, new PoolEntry({0})); - localDataPoolMap.emplace(IMTQ::COIL_Y_CURRENT, new PoolEntry({0})); - localDataPoolMap.emplace(IMTQ::COIL_Z_CURRENT, new PoolEntry({0})); - localDataPoolMap.emplace(IMTQ::COIL_X_TEMPERATURE, new PoolEntry({0})); - localDataPoolMap.emplace(IMTQ::COIL_Y_TEMPERATURE, new PoolEntry({0})); - localDataPoolMap.emplace(IMTQ::COIL_Z_TEMPERATURE, new PoolEntry({0})); - localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry({0})); + /** Entries of housekeeping dataset */ + localDataPoolMap.emplace(imtq::STATUS_BYTE_MODE, &statusMode); + localDataPoolMap.emplace(imtq::STATUS_BYTE_CONF, &statusConfig); + localDataPoolMap.emplace(imtq::STATUS_BYTE_ERROR, &statusError); + localDataPoolMap.emplace(imtq::STATUS_BYTE_UPTIME, &statusUptime); + localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::ANALOG_VOLTAGE_MV, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::DIGITAL_CURRENT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::ANALOG_CURRENT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::COIL_X_CURRENT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::COIL_Y_CURRENT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::COIL_Z_CURRENT, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::COIL_X_TEMPERATURE, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::COIL_Y_TEMPERATURE, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::COIL_Z_TEMPERATURE, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::MCU_TEMPERATURE, new PoolEntry({0})); - localDataPoolMap.emplace(IMTQ::DIPOLES_X, &dipoleXEntry); - localDataPoolMap.emplace(IMTQ::DIPOLES_Y, &dipoleYEntry); - localDataPoolMap.emplace(IMTQ::DIPOLES_Z, &dipoleZEntry); - localDataPoolMap.emplace(IMTQ::CURRENT_TORQUE_DURATION, &torqueDurationEntry); + localDataPoolMap.emplace(imtq::DIPOLES_X, &dipoleXEntry); + localDataPoolMap.emplace(imtq::DIPOLES_Y, &dipoleYEntry); + localDataPoolMap.emplace(imtq::DIPOLES_Z, &dipoleZEntry); + localDataPoolMap.emplace(imtq::CURRENT_TORQUE_DURATION, &torqueDurationEntry); /** Entries of calibrated MTM measurement dataset */ - localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, &mgmCalEntry); - localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::MGM_CAL_NT, &mgmCalEntry); + localDataPoolMap.emplace(imtq::ACTUATION_CAL_STATUS, new PoolEntry({0})); /** Entries of raw MTM measurement dataset */ - localDataPoolMap.emplace(IMTQ::MTM_RAW, new PoolEntry(3)); - localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry({0})); + localDataPoolMap.emplace(imtq::MTM_RAW, new PoolEntry(3)); + 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})); + 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})); + 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})); + 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})); + 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})); + 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::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::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::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::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::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::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})); + 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})); + 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})); + 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})); + 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})); + 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})); + 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})); + 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})); poolManager.subscribeForDiagPeriodicPacket( - subdp::DiagnosticsHkPeriodicParams(engHkDataset.getSid(), false, 10.0)); + subdp::DiagnosticsHkPeriodicParams(hkDatasetNoTorque.getSid(), false, 10.0)); + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(hkDatasetWithTorque.getSid(), false, 10.0)); + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(rawMtmNoTorque.getSid(), false, 10.0)); + poolManager.subscribeForDiagPeriodicPacket( + subdp::DiagnosticsHkPeriodicParams(rawMtmWithTorque.getSid(), false, 10.0)); poolManager.subscribeForDiagPeriodicPacket( subdp::DiagnosticsHkPeriodicParams(calMtmMeasurementSet.getSid(), false, 10.0)); - poolManager.subscribeForDiagPeriodicPacket( - subdp::DiagnosticsHkPeriodicParams(rawMtmMeasurementSet.getSid(), false, 10.0)); return DeviceHandlerBase::initializeLocalDataPool(localDataPoolMap, poolManager); } 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: + case imtq::cmdIds::POS_X_SELF_TEST: + case imtq::cmdIds::NEG_X_SELF_TEST: + case imtq::cmdIds::POS_Y_SELF_TEST: + case imtq::cmdIds::NEG_Y_SELF_TEST: + case imtq::cmdIds::POS_Z_SELF_TEST: + case imtq::cmdIds::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 UNEXPECTED_SELF_TEST_REPLY; } return returnvalue::OK; } -ReturnValue_t ImtqHandler::parseStatusByte(const uint8_t* packet) { +ReturnValue_t ImtqHandler::parseStatusByte(imtq::CC::CC command, const uint8_t* packet) { uint8_t cmdErrorField = *(packet + 1) & 0xF; + if (cmdErrorField == 0) { + return returnvalue::OK; + } + sif::error << std::hex; switch (cmdErrorField) { - case 0: - return returnvalue::OK; case 1: - sif::error << "IMTQHandler::parseStatusByte: Command rejected without reason" << std::endl; - return REJECTED_WITHOUT_REASON; + sif::error << "IMTQ::parseStatusByte: Command 0x" << std::setw(2) << command + << " rejected without reason" << std::endl; + return imtq::REJECTED_WITHOUT_REASON; case 2: - sif::error << "IMTQHandler::parseStatusByte: Command has invalid command code" << std::endl; - return INVALID_COMMAND_CODE; + sif::error << "IMTQ::parseStatusByte: Command 0x" << std::setw(2) << command + << " has invalid command code" << std::endl; + return imtq::INVALID_COMMAND_CODE; case 3: - sif::error << "IMTQHandler::parseStatusByte: Command has missing parameter" << std::endl; - return PARAMETER_MISSING; + sif::error << "IMTQ::parseStatusByte: Command 0x" << std::setw(2) << command + << " has missing parameter" << std::endl; + return imtq::PARAMETER_MISSING; case 4: - sif::error << "IMTQHandler::parseStatusByte: Command has invalid parameter" << std::endl; - return PARAMETER_INVALID; + sif::error << "IMTQ::parseStatusByte: Command 0x" << std::setw(2) << command + << " has invalid parameter" << std::endl; + return imtq::PARAMETER_INVALID; case 5: - sif::error << "IMTQHandler::parseStatusByte: CC unavailable" << std::endl; - return CC_UNAVAILABLE; + sif::error << "IMTQ::parseStatusByte: CC 0x" << std::setw(2) << " unavailable" << std::endl; + return imtq::CC_UNAVAILABLE; case 7: - sif::error << "IMTQHandler::parseStatusByte: IMQT replied internal processing error" - << std::endl; - return INTERNAL_PROCESSING_ERROR; + sif::error << "IMTQ::parseStatusByte: IMTQ replied internal processing error" << std::endl; + return imtq::INTERNAL_PROCESSING_ERROR; default: - sif::error << "IMTQHandler::parseStatusByte: CMD Error field contains unknown error code " - << cmdErrorField << std::endl; - return CMD_ERR_UNKNOWN; + sif::error << "IMTQ::parseStatusByte: CMD Error field contains unknown error code 0x" + << static_cast(cmdErrorField) << std::endl; + return imtq::CMD_ERR_UNKNOWN; } } -void ImtqHandler::fillEngHkDataset(const uint8_t* packet) { - PoolReadGuard rg(&engHkDataset); +void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* packet) { + PoolReadGuard rg(&hkDataset); uint8_t offset = 2; - engHkDataset.digitalVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset); + hkDataset.digitalVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; - engHkDataset.analogVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset); + hkDataset.analogVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset); offset += 2; - engHkDataset.digitalCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; + hkDataset.digitalCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; - engHkDataset.analogCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; + hkDataset.analogCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; - engHkDataset.coilXCurrentmA = + hkDataset.coilXCurrentmA = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; - engHkDataset.coilYCurrentmA = + hkDataset.coilYCurrentmA = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; - engHkDataset.coilZCurrentmA = + hkDataset.coilZCurrentmA = static_cast(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1; offset += 2; - engHkDataset.coilXTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); + hkDataset.coilXTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); offset += 2; - engHkDataset.coilYTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); + hkDataset.coilYTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); offset += 2; - engHkDataset.coilZTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); + hkDataset.coilZTemperature = (*(packet + offset + 1) << 8 | *(packet + offset)); offset += 2; size_t dummy = 2; - SerializeAdapter::deSerialize(&engHkDataset.mcuTemperature.value, packet + offset, &dummy, + SerializeAdapter::deSerialize(&hkDataset.mcuTemperature.value, packet + offset, &dummy, SerializeIF::Endianness::LITTLE); - engHkDataset.setValidity(true, true); + hkDataset.setValidity(true, true); if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 - sif::info << "IMTQ digital voltage: " << engHkDataset.digitalVoltageMv << " mV" << std::endl; - sif::info << "IMTQ analog voltage: " << engHkDataset.analogVoltageMv << " mV" << std::endl; - sif::info << "IMTQ digital current: " << engHkDataset.digitalCurrentmA << " mA" << std::endl; - sif::info << "IMTQ analog current: " << engHkDataset.analogCurrentmA << " mA" << std::endl; - sif::info << "IMTQ coil X current: " << engHkDataset.coilXCurrentmA << " mA" << std::endl; - sif::info << "IMTQ coil Y current: " << engHkDataset.coilYCurrentmA << " mA" << std::endl; - sif::info << "IMTQ coil Z current: " << engHkDataset.coilZCurrentmA << " mA" << std::endl; - sif::info << "IMTQ coil X temperature: " << engHkDataset.coilXTemperature << " °C" << std::endl; - sif::info << "IMTQ coil Y temperature: " << engHkDataset.coilYTemperature << " °C" << std::endl; - sif::info << "IMTQ coil Z temperature: " << engHkDataset.coilZTemperature << " °C" << std::endl; - sif::info << "IMTQ coil MCU temperature: " << engHkDataset.mcuTemperature << " °C" << std::endl; + sif::info << "IMTQ digital voltage: " << hkDataset.digitalVoltageMv << " mV" << std::endl; + sif::info << "IMTQ analog voltage: " << hkDataset.analogVoltageMv << " mV" << std::endl; + sif::info << "IMTQ digital current: " << hkDataset.digitalCurrentmA << " mA" << std::endl; + sif::info << "IMTQ analog current: " << hkDataset.analogCurrentmA << " mA" << std::endl; + sif::info << "IMTQ coil X current: " << hkDataset.coilXCurrentmA << " mA" << std::endl; + sif::info << "IMTQ coil Y current: " << hkDataset.coilYCurrentmA << " mA" << std::endl; + sif::info << "IMTQ coil Z current: " << hkDataset.coilZCurrentmA << " mA" << std::endl; + sif::info << "IMTQ coil X temperature: " << hkDataset.coilXTemperature << " °C" << std::endl; + sif::info << "IMTQ coil Y temperature: " << hkDataset.coilYTemperature << " °C" << std::endl; + sif::info << "IMTQ coil Z temperature: " << hkDataset.coilZTemperature << " °C" << std::endl; + sif::info << "IMTQ coil MCU temperature: " << hkDataset.mcuTemperature << " °C" << std::endl; #endif } } void ImtqHandler::setToGoToNormal(bool enable) { this->goToNormalMode = enable; } -void ImtqHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) { - if (wiretappingMode == RAW) { - /* Data already sent in doGetRead() */ - return; - } - - DeviceReplyMap::iterator iter = deviceReplyMap.find(replyId); - if (iter == deviceReplyMap.end()) { - sif::debug << "IMTQHandler::handleDeviceTM: Unknown reply id" << std::endl; - return; - } - MessageQueueId_t queueId = iter->second.command->second.sendReplyTo; - - if (queueId == NO_COMMANDER) { - return; - } - - ReturnValue_t result = actionHelper.reportData(queueId, replyId, data, dataSize); - if (result != returnvalue::OK) { - sif::debug << "IMTQHandler::handleDeviceTM: Failed to report data" << std::endl; - return; - } -} - -void ImtqHandler::handleGetCommandedDipoleReply(const uint8_t* packet) { - uint8_t tmData[6]; - /* Switching endianess of received dipole values */ - tmData[0] = *(packet + 3); - tmData[1] = *(packet + 2); - tmData[2] = *(packet + 5); - tmData[3] = *(packet + 4); - tmData[4] = *(packet + 7); - tmData[5] = *(packet + 6); - handleDeviceTM(tmData, sizeof(tmData), IMTQ::GET_COMMANDED_DIPOLE); -} - void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) { PoolReadGuard rg(&calMtmMeasurementSet); calMtmMeasurementSet.setValidity(true, true); @@ -880,7 +848,7 @@ void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) { } void ImtqHandler::fillRawMtmDataset(const uint8_t* packet) { - PoolReadGuard rg(&rawMtmMeasurementSet); + PoolReadGuard rg(&rawMtmNoTorque); unsigned int offset = 2; size_t deSerLen = 16; const uint8_t* dataStart = packet + offset; @@ -908,21 +876,18 @@ void ImtqHandler::fillRawMtmDataset(const uint8_t* packet) { if (res != returnvalue::OK) { return; } - rawMtmMeasurementSet.mtmRawNt[0] = xRaw * 7.5; - rawMtmMeasurementSet.mtmRawNt[1] = yRaw * 7.5; - rawMtmMeasurementSet.mtmRawNt[2] = zRaw * 7.5; - rawMtmMeasurementSet.coilActuationStatus = static_cast(coilActStatus); - rawMtmMeasurementSet.setValidity(true, true); + rawMtmNoTorque.mtmRawNt[0] = xRaw * 7.5; + rawMtmNoTorque.mtmRawNt[1] = yRaw * 7.5; + rawMtmNoTorque.mtmRawNt[2] = zRaw * 7.5; + rawMtmNoTorque.coilActuationStatus = static_cast(coilActStatus); + rawMtmNoTorque.setValidity(true, true); if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 - sif::info << "IMTQ raw MTM measurement X: " << rawMtmMeasurementSet.mtmRawNt[0] << " nT" - << std::endl; - sif::info << "IMTQ raw MTM measurement Y: " << rawMtmMeasurementSet.mtmRawNt[1] << " nT" - << std::endl; - sif::info << "IMTQ raw MTM measurement Z: " << rawMtmMeasurementSet.mtmRawNt[2] << " nT" - << std::endl; + sif::info << "IMTQ raw MTM measurement X: " << rawMtmNoTorque.mtmRawNt[0] << " nT" << std::endl; + sif::info << "IMTQ raw MTM measurement Y: " << rawMtmNoTorque.mtmRawNt[1] << " nT" << std::endl; + sif::info << "IMTQ raw MTM measurement Z: " << rawMtmNoTorque.mtmRawNt[2] << " nT" << std::endl; sif::info << "IMTQ coil actuation status during MTM measurement: " - << (unsigned int)rawMtmMeasurementSet.coilActuationStatus.value << std::endl; + << (unsigned int)rawMtmNoTorque.coilActuationStatus.value << std::endl; #endif } } @@ -931,28 +896,28 @@ 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: { + switch (*(packet + imtq::MAIN_STEP_OFFSET)) { + case imtq::selfTest::step::X_POSITIVE: { handlePositiveXSelfTestReply(packet); break; } - case IMTQ::SELF_TEST_STEPS::X_NEGATIVE: { + case imtq::selfTest::step::X_NEGATIVE: { handleNegativeXSelfTestReply(packet); break; } - case IMTQ::SELF_TEST_STEPS::Y_POSITIVE: { + case imtq::selfTest::step::Y_POSITIVE: { handlePositiveYSelfTestReply(packet); break; } - case IMTQ::SELF_TEST_STEPS::Y_NEGATIVE: { + case imtq::selfTest::step::Y_NEGATIVE: { handleNegativeYSelfTestReply(packet); break; } - case IMTQ::SELF_TEST_STEPS::Z_POSITIVE: { + case imtq::selfTest::step::Z_POSITIVE: { handlePositiveZSelfTestReply(packet); break; } - case IMTQ::SELF_TEST_STEPS::Z_NEGATIVE: { + case imtq::selfTest::step::Z_NEGATIVE: { handleNegativeZSelfTestReply(packet); break; } @@ -2249,80 +2214,86 @@ void ImtqHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) { return; } - if (errorByte & IMTQ::I2C_FAILURE_MASK) { + 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) { + 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) { + 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) { + 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) { + 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) { + 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) { + 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) { + 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; } } -void ImtqHandler::doSendRead() { - TaskFactory::delayTask(1); - DeviceHandlerBase::doSendRead(); +void ImtqHandler::fillSystemStateIntoDataset(const uint8_t* packet) { + PoolReadGuard pg(&statusSet); + statusSet.statusByteMode.value = packet[2]; + statusSet.statusByteError.value = packet[3]; + statusSet.statusByteConfig.value = packet[4]; + size_t dummy = 0; + SerializeAdapter::deSerialize(&statusSet.statusByteUptime.value, packet + 5, &dummy, + SerializeIF::Endianness::LITTLE); + statusSet.setValidity(true, true); } std::string ImtqHandler::makeStepString(const uint8_t step) { std::string stepString(""); switch (step) { - case IMTQ::SELF_TEST_STEPS::INIT: + case imtq::selfTest::step::INIT: stepString = std::string("INIT"); break; - case IMTQ::SELF_TEST_STEPS::X_POSITIVE: + case imtq::selfTest::step::X_POSITIVE: stepString = std::string("+X"); break; - case IMTQ::SELF_TEST_STEPS::X_NEGATIVE: + case imtq::selfTest::step::X_NEGATIVE: stepString = std::string("-X"); break; - case IMTQ::SELF_TEST_STEPS::Y_POSITIVE: + case imtq::selfTest::step::Y_POSITIVE: stepString = std::string("+Y"); break; - case IMTQ::SELF_TEST_STEPS::Y_NEGATIVE: + case imtq::selfTest::step::Y_NEGATIVE: stepString = std::string("-Y"); break; - case IMTQ::SELF_TEST_STEPS::Z_POSITIVE: + case imtq::selfTest::step::Z_POSITIVE: stepString = std::string("+Z"); break; - case IMTQ::SELF_TEST_STEPS::Z_NEGATIVE: + case imtq::selfTest::step::Z_NEGATIVE: stepString = std::string("-Z"); break; - case IMTQ::SELF_TEST_STEPS::FINA: + case imtq::selfTest::step::FINA: stepString = std::string("FINA"); break; default: diff --git a/mission/devices/ImtqHandler.h b/mission/devices/ImtqHandler.h index cfad9d7f..a674f85b 100644 --- a/mission/devices/ImtqHandler.h +++ b/mission/devices/ImtqHandler.h @@ -2,7 +2,7 @@ #define MISSION_DEVICES_IMTQHANDLER_H_ #include -#include +#include #include #include "events/subsystemIdRanges.h" @@ -23,7 +23,6 @@ class ImtqHandler : public DeviceHandlerBase { void setPollingMode(NormalPollingMode pollMode); - void doSendRead() override; /** * @brief Sets mode to MODE_NORMAL. Can be used for debugging. */ @@ -32,6 +31,7 @@ class ImtqHandler : public DeviceHandlerBase { void setDebugMode(bool enable); protected: + ReturnValue_t performOperation(uint8_t opCode); void doStartUp() override; void doShutDown() override; ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t* id) override; @@ -42,7 +42,6 @@ class ImtqHandler : public DeviceHandlerBase { ReturnValue_t scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) override; 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, @@ -50,19 +49,6 @@ class ImtqHandler : public DeviceHandlerBase { ReturnValue_t getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) override; private: - static const uint8_t INTERFACE_ID = CLASS_ID::IMTQ_HANDLER; - - static const ReturnValue_t INVALID_COMMAND_CODE = MAKE_RETURN_CODE(0xA0); - static const ReturnValue_t PARAMETER_MISSING = MAKE_RETURN_CODE(0xA1); - static const ReturnValue_t PARAMETER_INVALID = MAKE_RETURN_CODE(0xA2); - static const ReturnValue_t CC_UNAVAILABLE = MAKE_RETURN_CODE(0xA3); - 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 @@ -97,49 +83,43 @@ class ImtqHandler : public DeviceHandlerBase { //! 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::DipoleActuationSet dipoleSet; - IMTQ::PosXSelfTestSet posXselfTestDataset; - IMTQ::NegXSelfTestSet negXselfTestDataset; - IMTQ::PosYSelfTestSet posYselfTestDataset; - IMTQ::NegYSelfTestSet negYselfTestDataset; - IMTQ::PosZSelfTestSet posZselfTestDataset; - IMTQ::NegZSelfTestSet negZselfTestDataset; + imtq::StatusDataset statusSet; + imtq::DipoleActuationSet dipoleSet; + imtq::RawMtmMeasurementNoTorque rawMtmNoTorque; + imtq::HkDatasetNoTorque hkDatasetNoTorque; + + imtq::RawMtmMeasurementWithTorque rawMtmWithTorque; + imtq::HkDatasetWithTorque hkDatasetWithTorque; + + imtq::CalibratedMtmMeasurementSet calMtmMeasurementSet; + imtq::PosXSelfTestSet posXselfTestDataset; + imtq::NegXSelfTestSet negXselfTestDataset; + imtq::PosYSelfTestSet posYselfTestDataset; + imtq::NegYSelfTestSet negYselfTestDataset; + imtq::PosZSelfTestSet posZselfTestDataset; + imtq::NegZSelfTestSet negZselfTestDataset; NormalPollingMode pollingMode = NormalPollingMode::UNCALIBRATED; + PoolEntry statusMode = PoolEntry({0}); + PoolEntry statusError = PoolEntry({0}); + PoolEntry statusConfig = PoolEntry({0}); + PoolEntry statusUptime = PoolEntry({0}); + PoolEntry mgmCalEntry = PoolEntry(3); PoolEntry dipoleXEntry = PoolEntry(0, false); PoolEntry dipoleYEntry = PoolEntry(0, false); PoolEntry dipoleZEntry = PoolEntry(0, false); PoolEntry torqueDurationEntry = PoolEntry(0, false); - // Hardcoded to default integration time of 10 ms. - // SHOULDDO: Support for other integration times - Countdown integrationTimeCd = Countdown(10); - power::Switch_t switcher = power::NO_SWITCH; - uint8_t commandBuffer[IMTQ::MAX_COMMAND_SIZE]; + uint8_t commandBuffer[imtq::MAX_COMMAND_SIZE]; bool goToNormalMode = false; bool debugMode = false; + bool specialRequestActive = false; + bool firstReplyCycle = true; - enum class CommunicationStep { - GET_ENG_HK_DATA, - START_MTM_MEASUREMENT, - GET_CAL_MTM_MEASUREMENT, - GET_RAW_MTM_MEASUREMENT, - DIPOLE_ACTUATION - }; - - 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; + imtq::RequestType requestStep = imtq::RequestType::MEASURE_NO_ACTUATION; /** * @brief In case of a status reply to a single axis self test command, this function @@ -155,7 +135,7 @@ class ImtqHandler : public DeviceHandlerBase { * * @return The return code derived from the received status byte. */ - ReturnValue_t parseStatusByte(const uint8_t* packet); + ReturnValue_t parseStatusByte(imtq::CC::CC command, const uint8_t* packet); /** * @brief This function fills the engineering housekeeping dataset with the received data. @@ -163,23 +143,9 @@ class ImtqHandler : public DeviceHandlerBase { * @param packet Pointer to the received data. * */ - void fillEngHkDataset(const uint8_t* packet); + void fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* packet); - /** - * @brief This function sends a command reply to the requesting queue. - * - * @param data Pointer to the data to send. - * @param dataSize Size of the data to send. - * @param relplyId Reply id which will be inserted at the beginning of the action message. - */ - void handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId); - - /** - * @brief This function handles the reply containing the commanded dipole. - * - * @param packet Pointer to the reply data. - */ - void handleGetCommandedDipoleReply(const uint8_t* packet); + void fillSystemStateIntoDataset(const uint8_t* packet); /** * @brief This function parses the reply containing the calibrated MTM measurement and writes @@ -212,7 +178,7 @@ class ImtqHandler : public DeviceHandlerBase { void handlePositiveZSelfTestReply(const uint8_t* packet); void handleNegativeZSelfTestReply(const uint8_t* packet); - ReturnValue_t buildDipoleActuationCommand(); + // ReturnValue_t buildDipoleActuationCommand(); /** * @brief This function checks the error byte of a self test measurement. * diff --git a/mission/devices/devicedefinitions/CMakeLists.txt b/mission/devices/devicedefinitions/CMakeLists.txt index cee48ea8..b2394b2a 100644 --- a/mission/devices/devicedefinitions/CMakeLists.txt +++ b/mission/devices/devicedefinitions/CMakeLists.txt @@ -1 +1,2 @@ -target_sources(${LIB_EIVE_MISSION} PRIVATE ScexDefinitions.cpp rwHelpers.cpp) +target_sources(${LIB_EIVE_MISSION} PRIVATE ScexDefinitions.cpp rwHelpers.cpp + imtqHelpers.cpp) diff --git a/mission/devices/devicedefinitions/imtqHelpers.cpp b/mission/devices/devicedefinitions/imtqHelpers.cpp new file mode 100644 index 00000000..758cfe23 --- /dev/null +++ b/mission/devices/devicedefinitions/imtqHelpers.cpp @@ -0,0 +1,49 @@ +#include "imtqHelpers.h" + +size_t imtq::getReplySize(CC::CC cc, size_t* optSecondSize) { + switch (cc) { + // Software reset is a bit special and can also cause a I2C NAK because + // the device might be reset at that moment. Otherwise, 2 bytes should be returned + case (CC::CC::SOFTWARE_RESET): { + if (optSecondSize != nullptr) { + *optSecondSize = 0; + } + return 2; + } + case (CC::CC::START_ACTUATION_DIPOLE): + case (CC::CC::SELF_TEST_CMD): + case (CC::CC::START_MTM_MEASUREMENT): { + return 2; + } + case (CC::CC::GET_SYSTEM_STATE): { + return 9; + } + case (CC::CC::GET_RAW_MTM_MEASUREMENT): + case (CC::CC::GET_CAL_MTM_MEASUREMENT): { + return 15; + } + case (CC::CC::GET_COIL_CURRENT): + case (CC::CC::GET_COMMANDED_DIPOLE): + case (CC::CC::GET_COIL_TEMPERATURES): { + return 8; + } + case (CC::CC::GET_SELF_TEST_RESULT): { + // Can also be 360 for the all axes self-test! + if (optSecondSize != nullptr) { + *optSecondSize = 360; + } + return 120; + } + case (CC::CC::GET_RAW_HK_DATA): + case (CC::CC::GET_ENG_HK_DATA): { + return 24; + } + case (CC::CC::GET_PARAM): { + return imtq::replySize::MAX_SET_GET_PARAM_LEN; + } + default: { + return 0; + } + } + return 0; +} diff --git a/mission/devices/devicedefinitions/imtqHandlerDefinitions.h b/mission/devices/devicedefinitions/imtqHelpers.h similarity index 75% rename from mission/devices/devicedefinitions/imtqHandlerDefinitions.h rename to mission/devices/devicedefinitions/imtqHelpers.h index c3bc3d36..125b80c9 100644 --- a/mission/devices/devicedefinitions/imtqHandlerDefinitions.h +++ b/mission/devices/devicedefinitions/imtqHelpers.h @@ -1,23 +1,60 @@ #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_ #define MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_ +#include #include #include +#include class ImtqHandler; -namespace IMTQ { +namespace imtq { -static const DeviceCommandId_t NONE = 0x0; -static const DeviceCommandId_t GET_ENG_HK_DATA = 0x1; +enum ComStep : uint8_t { + DHB_OP = 0, + START_MEASURE_SEND = 1, + START_MEASURE_GET = 2, + READ_MEASURE_SEND = 3, + READ_MEASURE_GET = 4, + START_ACTUATE_SEND = 5, + START_ACTUATE_GET = 6, + READ_ACTUATE_SEND = 7, + READ_ACTUATE_GET = 8, +}; + +enum class RequestType : uint8_t { MEASURE_NO_ACTUATION, ACTUATE }; + +enum class SpecialRequest : uint8_t { + NONE = 0, + DO_SELF_TEST_POS_X = 1, + DO_SELF_TEST_NEG_X = 2, + DO_SELF_TEST_POS_Y = 3, + DO_SELF_TEST_NEG_Y = 4, + DO_SELF_TEST_POS_Z = 5, + DO_SELF_TEST_NEG_Z = 6, + GET_SELF_TEST_RESULT = 7 +}; + +static const uint8_t INTERFACE_ID = CLASS_ID::IMTQ_HANDLER; + +static constexpr ReturnValue_t INVALID_COMMAND_CODE = MAKE_RETURN_CODE(0); +static constexpr ReturnValue_t MGM_MEASUREMENT_LOW_LEVEL_ERROR = MAKE_RETURN_CODE(1); +static constexpr ReturnValue_t ACTUATE_CMD_LOW_LEVEL_ERROR = MAKE_RETURN_CODE(2); +static const ReturnValue_t PARAMETER_MISSING = MAKE_RETURN_CODE(3); +static const ReturnValue_t PARAMETER_INVALID = MAKE_RETURN_CODE(4); +static const ReturnValue_t CC_UNAVAILABLE = MAKE_RETURN_CODE(5); +static const ReturnValue_t INTERNAL_PROCESSING_ERROR = MAKE_RETURN_CODE(6); +static const ReturnValue_t REJECTED_WITHOUT_REASON = MAKE_RETURN_CODE(7); +static const ReturnValue_t CMD_ERR_UNKNOWN = MAKE_RETURN_CODE(8); +//! [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); + +namespace cmdIds { + +static constexpr DeviceCommandId_t REQUEST = 0x70; +static constexpr DeviceCommandId_t REPLY = 0x71; static const DeviceCommandId_t START_ACTUATION_DIPOLE = 0x2; -static const DeviceCommandId_t GET_COMMANDED_DIPOLE = 0x3; -/** Generates new measurement of the magnetic field */ -static const DeviceCommandId_t START_MTM_MEASUREMENT = 0x4; -/** Requests the calibrated magnetometer measurement */ -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 POS_X_SELF_TEST = 0x7; static const DeviceCommandId_t NEG_X_SELF_TEST = 0x8; static const DeviceCommandId_t POS_Y_SELF_TEST = 0x9; @@ -26,36 +63,49 @@ 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; +} // namespace cmdIds + static const uint8_t POINTER_REG_SIZE = 1; enum SetIds : uint32_t { - ENG_HK = 1, - CAL_MGM = 2, - RAW_MGM = 3, - POS_X_TEST = 4, - NEG_X_TEST = 5, - POS_Y_TEST = 6, - NEG_Y_TEST = 7, - POS_Z_TEST = 8, - NEG_Z_TEST = 9, - DIPOLES = 10 + ENG_HK_NO_TORQUE = 1, + RAW_MTM_NO_TORQUE = 2, + ENG_HK_SET_WITH_TORQUE = 3, + RAW_MTM_WITH_TORQUE = 4, + STATUS_SET = 5, + DIPOLES = 6, + + CAL_MTM_SET = 9, + POSITIVE_X_TEST = 10, + NEGATIVE_X_TEST = 11, + POSITIVE_Y_TEST = 12, + NEGATIVE_Y_TEST = 13, + POSITIVE_Z_TEST = 14, + NEGATIVE_Z_TEST = 15, }; -static const uint8_t SIZE_ENG_HK_COMMAND = 1; -static const uint8_t SIZE_STATUS_REPLY = 2; -static const uint8_t SIZE_ENG_HK_DATA_REPLY = 24; -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; +namespace replySize { -static const uint16_t MAX_REPLY_SIZE = SIZE_SELF_TEST_RESULTS; -static const uint8_t MAX_COMMAND_SIZE = 9; +static constexpr uint8_t GET_TEMP_REPLY_SIZE = 2; +static constexpr uint8_t CFGR_CMD_SIZE = 3; +static constexpr size_t DEFAULT_MIN_LEN = 2; +static constexpr size_t STATUS_REPLY = DEFAULT_MIN_LEN; +static constexpr size_t ENG_HK_DATA_REPLY = 24; +static constexpr size_t GET_COMMANDED_DIPOLE_REPLY = 8; +static constexpr size_t MAX_SET_GET_PARAM_LEN = 12; +static constexpr size_t SYSTEM_STATE = 9; +static constexpr size_t CAL_MTM_MEASUREMENT = 15; +static constexpr size_t RAW_MTM_MEASUREMENT = 15; +static constexpr size_t SELF_TEST_RESULTS = 120; +static constexpr size_t SELF_TEST_RESULTS_ALL_AXES = 360; + +} // namespace replySize + +static const uint16_t MAX_REPLY_SIZE = replySize::SELF_TEST_RESULTS_ALL_AXES; +static const uint8_t MAX_COMMAND_SIZE = 16; /** Define entries in IMTQ specific dataset */ -static const uint8_t ENG_HK_SET_POOL_ENTRIES = 11; +static const uint8_t HK_SET_POOL_ENTRIES = 20; static const uint8_t CAL_MTM_POOL_ENTRIES = 4; static const uint8_t SELF_TEST_DATASET_ENTRIES = 104; @@ -72,45 +122,75 @@ static const uint8_t INVALID_ERROR_BYTE = static const uint8_t MAIN_STEP_OFFSET = 43; +// Command Code +namespace CC { + /** * Command code definitions. Each command or reply of an IMTQ request will begin with one of * the following command codes. */ -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; -static const uint8_t PAST_AVAILABLE_RESPONSE_BYTES = 0xff; -}; // namespace CC +enum CC : uint8_t { + START_MTM_MEASUREMENT = 0x4, + START_ACTUATION_DIPOLE = 0x6, + SELF_TEST_CMD = 0x8, + GET_SYSTEM_STATE = 0x41, + GET_RAW_MTM_MEASUREMENT = 0x42, + GET_CAL_MTM_MEASUREMENT = 0x43, + GET_COIL_CURRENT = 0x44, + GET_COIL_TEMPERATURES = 0x45, + GET_COMMANDED_DIPOLE = 0x46, + GET_SELF_TEST_RESULT = 0x47, + GET_RAW_HK_DATA = 0x49, + GET_ENG_HK_DATA = 0x4A, + GET_PARAM = 0x81, + SET_PARAM = 0x82, + SOFTWARE_RESET = 0xAA, + PAST_AVAILABLE_RESPONSE_BYTES = 0xff +}; -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_AXIS +} // namespace CC -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; -} // namespace SELF_TEST_STEPS +size_t getReplySize(CC::CC cc, size_t* optSecondSize = nullptr); + +namespace mode { +enum Mode : uint8_t { IDLE = 0, SELF_TEST = 1, DETUMBLE = 2 }; +} + +namespace selfTest { + +enum Axis : uint8_t { + + ALL = 0x0, + X_POSITIVE = 0x1, + X_NEGATIVE = 0x2, + Y_POSITIVE = 0x3, + Y_NEGATIVE = 0x4, + Z_POSITIVE = 0x5, + Z_NEGATIVE = 0x6, +}; + +namespace step { + +enum Step : uint8_t { + + INIT = 0x0, + X_POSITIVE = 0x1, + X_NEGATIVE = 0x2, + Y_POSITIVE = 0x3, + Y_NEGATIVE = 0x4, + Z_POSITIVE = 0x5, + Z_NEGATIVE = 0x6, + FINA = 0x7 +}; +} +} // namespace selfTest enum PoolIds : lp_id_t { + STATUS_BYTE_MODE, + STATUS_BYTE_ERROR, + STATUS_BYTE_CONF, + STATUS_BYTE_UPTIME, + DIGITAL_VOLTAGE_MV, ANALOG_VOLTAGE_MV, DIGITAL_CURRENT, @@ -384,12 +464,23 @@ enum PoolIds : lp_id_t { FINA_NEG_Z_COIL_Z_TEMPERATURE, }; -class EngHkDataset : public StaticLocalDataSet { +class StatusDataset : public StaticLocalDataSet<4> { public: - EngHkDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, IMTQ::SetIds::ENG_HK) {} + StatusDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, imtq::SetIds::STATUS_SET) {} + // Status byte variables + lp_var_t statusByteMode = lp_var_t(sid.objectId, STATUS_BYTE_MODE, this); + lp_var_t statusByteError = lp_var_t(sid.objectId, STATUS_BYTE_ERROR, this); + lp_var_t statusByteConfig = lp_var_t(sid.objectId, STATUS_BYTE_CONF, this); + lp_var_t statusByteUptime = lp_var_t(sid.objectId, STATUS_BYTE_UPTIME, this); +}; - EngHkDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::ENG_HK)) {} +class HkDataset : public StaticLocalDataSet { + public: + HkDataset(HasLocalDataPoolIF* owner, uint32_t setId) : StaticLocalDataSet(owner, setId) {} + HkDataset(object_id_t objectId, uint32_t setId) : StaticLocalDataSet(sid_t(objectId, setId)) {} + + // Engineering HK variables 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); @@ -404,16 +495,27 @@ class EngHkDataset : public StaticLocalDataSet { lp_var_t mcuTemperature = lp_var_t(sid.objectId, MCU_TEMPERATURE, this); }; +class HkDatasetNoTorque : public HkDataset { + public: + HkDatasetNoTorque(HasLocalDataPoolIF* owner) : HkDataset(owner, imtq::SetIds::ENG_HK_NO_TORQUE) {} +}; + +class HkDatasetWithTorque : public HkDataset { + public: + HkDatasetWithTorque(HasLocalDataPoolIF* owner) + : HkDataset(owner, imtq::SetIds::ENG_HK_SET_WITH_TORQUE) {} +}; /** + * * @brief This dataset holds the last calibrated MTM measurement. */ class CalibratedMtmMeasurementSet : public StaticLocalDataSet { public: CalibratedMtmMeasurementSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, IMTQ::SetIds::CAL_MGM) {} + : StaticLocalDataSet(owner, imtq::SetIds::CAL_MTM_SET) {} CalibratedMtmMeasurementSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::CAL_MGM)) {} + : StaticLocalDataSet(sid_t(objectId, imtq::SetIds::CAL_MTM_SET)) {} /** The unit of all measurements is nT */ lp_vec_t mgmXyz = lp_vec_t(sid.objectId, MGM_CAL_NT, this); @@ -427,11 +529,11 @@ class CalibratedMtmMeasurementSet : public StaticLocalDataSet { public: - RawMtmMeasurementSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, IMTQ::SetIds::RAW_MGM) {} + RawMtmMeasurementSet(HasLocalDataPoolIF* owner, uint32_t setId) + : StaticLocalDataSet(owner, setId) {} - RawMtmMeasurementSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::RAW_MGM)) {} + RawMtmMeasurementSet(object_id_t objectId, uint32_t setId) + : StaticLocalDataSet(sid_t(objectId, setId)) {} /** The unit of all measurements is nT */ lp_vec_t mtmRawNt = lp_vec_t(sid.objectId, MTM_RAW, this); @@ -440,6 +542,21 @@ class RawMtmMeasurementSet : public StaticLocalDataSet { lp_var_t(sid.objectId, ACTUATION_RAW_STATUS, this); }; +class RawMtmMeasurementNoTorque : public RawMtmMeasurementSet { + public: + RawMtmMeasurementNoTorque(HasLocalDataPoolIF* owner) + : RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_NO_TORQUE) {} + RawMtmMeasurementNoTorque(object_id_t objectId) + : RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_NO_TORQUE) {} +}; +class RawMtmMeasurementWithTorque : public RawMtmMeasurementSet { + public: + RawMtmMeasurementWithTorque(HasLocalDataPoolIF* owner) + : RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_WITH_TORQUE) {} + RawMtmMeasurementWithTorque(object_id_t objectId) + : RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_WITH_TORQUE) {} +}; + /** * @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. @@ -482,9 +599,9 @@ class DipoleActuationSet : public StaticLocalDataSet<4> { public: DipoleActuationSet(HasLocalDataPoolIF& owner) - : StaticLocalDataSet(&owner, IMTQ::SetIds::DIPOLES) {} + : StaticLocalDataSet(&owner, imtq::SetIds::DIPOLES) {} DipoleActuationSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::DIPOLES)) {} + : StaticLocalDataSet(sid_t(objectId, imtq::SetIds::DIPOLES)) {} // Refresh torque command without changing any of the set dipoles. void refreshTorqueing(uint16_t durationMs_) { currentTorqueDurationMs = durationMs_; } @@ -534,10 +651,10 @@ class DipoleActuationSet : public StaticLocalDataSet<4> { class PosXSelfTestSet : public StaticLocalDataSet { public: PosXSelfTestSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, IMTQ::SetIds::POS_X_TEST) {} + : StaticLocalDataSet(owner, imtq::SetIds::POSITIVE_X_TEST) {} PosXSelfTestSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_X_TEST)) {} + : StaticLocalDataSet(sid_t(objectId, imtq::SetIds::POSITIVE_X_TEST)) {} /** INIT block */ lp_var_t initErr = lp_var_t(sid.objectId, INIT_POS_X_ERR, this); @@ -611,10 +728,10 @@ class PosXSelfTestSet : public StaticLocalDataSet { class NegXSelfTestSet : public StaticLocalDataSet { public: NegXSelfTestSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, IMTQ::SetIds::NEG_X_TEST) {} + : StaticLocalDataSet(owner, imtq::SetIds::NEGATIVE_X_TEST) {} NegXSelfTestSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_X_TEST)) {} + : StaticLocalDataSet(sid_t(objectId, imtq::SetIds::NEGATIVE_X_TEST)) {} /** INIT block */ lp_var_t initErr = lp_var_t(sid.objectId, INIT_NEG_X_ERR, this); @@ -688,10 +805,10 @@ class NegXSelfTestSet : public StaticLocalDataSet { class PosYSelfTestSet : public StaticLocalDataSet { public: PosYSelfTestSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, IMTQ::SetIds::POS_Y_TEST) {} + : StaticLocalDataSet(owner, imtq::SetIds::POSITIVE_Y_TEST) {} PosYSelfTestSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_Y_TEST)) {} + : StaticLocalDataSet(sid_t(objectId, imtq::SetIds::POSITIVE_Y_TEST)) {} /** INIT block */ lp_var_t initErr = lp_var_t(sid.objectId, INIT_POS_Y_ERR, this); @@ -765,10 +882,10 @@ class PosYSelfTestSet : public StaticLocalDataSet { class NegYSelfTestSet : public StaticLocalDataSet { public: NegYSelfTestSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, IMTQ::SetIds::NEG_Y_TEST) {} + : StaticLocalDataSet(owner, imtq::SetIds::NEGATIVE_Y_TEST) {} NegYSelfTestSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_Y_TEST)) {} + : StaticLocalDataSet(sid_t(objectId, imtq::SetIds::NEGATIVE_Y_TEST)) {} /** INIT block */ lp_var_t initErr = lp_var_t(sid.objectId, INIT_NEG_Y_ERR, this); @@ -842,10 +959,10 @@ class NegYSelfTestSet : public StaticLocalDataSet { class PosZSelfTestSet : public StaticLocalDataSet { public: PosZSelfTestSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, IMTQ::SetIds::POS_Z_TEST) {} + : StaticLocalDataSet(owner, imtq::SetIds::POSITIVE_Z_TEST) {} PosZSelfTestSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_Z_TEST)) {} + : StaticLocalDataSet(sid_t(objectId, imtq::SetIds::POSITIVE_Y_TEST)) {} /** INIT block */ lp_var_t initErr = lp_var_t(sid.objectId, INIT_POS_Z_ERR, this); @@ -919,10 +1036,10 @@ class PosZSelfTestSet : public StaticLocalDataSet { class NegZSelfTestSet : public StaticLocalDataSet { public: NegZSelfTestSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, IMTQ::SetIds::NEG_Z_TEST) {} + : StaticLocalDataSet(owner, imtq::SetIds::NEGATIVE_Z_TEST) {} NegZSelfTestSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_Z_TEST)) {} + : StaticLocalDataSet(sid_t(objectId, imtq::SetIds::NEGATIVE_Z_TEST)) {} /** INIT block */ lp_var_t initErr = lp_var_t(sid.objectId, INIT_NEG_Z_ERR, this); @@ -979,6 +1096,153 @@ class NegZSelfTestSet : public StaticLocalDataSet { lp_var_t(sid.objectId, FINA_NEG_Z_COIL_Z_TEMPERATURE, this); }; -} // namespace IMTQ +} // namespace imtq +struct ImtqRequest { + friend class ImtqHandler; + + public: + static constexpr size_t REQUEST_LEN = 10; + + ImtqRequest(const uint8_t* data, size_t maxSize) + : rawData(const_cast(data)), maxSize(maxSize) {} + + imtq::RequestType getRequestType() const { return static_cast(rawData[0]); } + + void setMeasureRequest(imtq::SpecialRequest specialRequest) { + rawData[0] = static_cast(imtq::RequestType::MEASURE_NO_ACTUATION); + rawData[1] = static_cast(specialRequest); + } + + void setActuateRequest(int16_t dipoleX, int16_t dipoleY, int16_t dipoleZ, + uint16_t torqueDuration) { + size_t dummy = 0; + rawData[0] = static_cast(imtq::RequestType::ACTUATE); + rawData[1] = static_cast(imtq::SpecialRequest::NONE); + uint8_t* serPtr = rawData + 2; + SerializeAdapter::serialize(&dipoleX, &serPtr, &dummy, maxSize, + SerializeIF::Endianness::MACHINE); + SerializeAdapter::serialize(&dipoleY, &serPtr, &dummy, maxSize, + SerializeIF::Endianness::MACHINE); + SerializeAdapter::serialize(&dipoleZ, &serPtr, &dummy, maxSize, + SerializeIF::Endianness::MACHINE); + SerializeAdapter::serialize(&torqueDuration, &serPtr, &dummy, maxSize, + SerializeIF::Endianness::MACHINE); + } + + uint8_t* startOfActuateDataPtr() { return rawData + 2; } + + int16_t* getDipoles() { return reinterpret_cast(rawData + 2); } + + uint16_t getTorqueDuration() { + uint8_t* data = rawData + 2 + 6; + uint16_t value = 0; + size_t dummy = 0; + SerializeAdapter::deSerialize(&value, data, &dummy, SerializeIF::Endianness::MACHINE); + return value; + } + + void setSpecialRequest(imtq::SpecialRequest specialRequest) { + rawData[1] = static_cast(specialRequest); + } + + imtq::SpecialRequest getSpecialRequest() const { + return static_cast(rawData[1]); + } + + private: + ImtqRequest(uint8_t* rawData, size_t maxLen) : rawData(rawData) { + if (rawData != nullptr) { + rawData[0] = static_cast(imtq::RequestType::MEASURE_NO_ACTUATION); + } + } + uint8_t* rawData; + size_t maxSize = 0; +}; + +struct ImtqRepliesDefault { + friend class ImtqPollingTask; + + public: + static constexpr size_t BASE_LEN = + imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::SYSTEM_STATE + 1 + + +imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::RAW_MTM_MEASUREMENT + 1 + + imtq::replySize::ENG_HK_DATA_REPLY + 1 + imtq::replySize::CAL_MTM_MEASUREMENT + 1; + ImtqRepliesDefault(const uint8_t* rawData) : rawData(const_cast(rawData)) { + initPointers(); + } + + uint8_t* getCalibMgmMeasurement() const { return calibMgmMeasurement + 1; } + bool wasCalibMgmMeasurementRead() const { return calibMgmMeasurement[0]; }; + + uint8_t* getEngHk() const { return engHk + 1; } + bool wasEngHkRead() const { return engHk[0]; }; + + uint8_t* getRawMgmMeasurement() const { return rawMgmMeasurement + 1; } + bool wasGetRawMgmMeasurementRead() const { return rawMgmMeasurement[0]; }; + + uint8_t* getSpecialRequest() const { return specialRequestReply + 1; } + bool wasSpecialRequestRead() const { return specialRequestReply[0]; } + uint8_t* getStartMtmMeasurement() const { return startMtmMeasurement + 1; } + bool wasStartMtmMeasurementRead() const { return startMtmMeasurement[0]; } + + uint8_t* getSwReset() const { return swReset + 1; } + + uint8_t* getSystemState() const { return systemState + 1; } + bool wasGetSystemStateRead() const { return systemState[0]; } + + private: + void initPointers() { + swReset = rawData; + systemState = swReset + imtq::replySize::DEFAULT_MIN_LEN + 1; + startMtmMeasurement = systemState + imtq::replySize::SYSTEM_STATE + 1; + rawMgmMeasurement = startMtmMeasurement + imtq::replySize::DEFAULT_MIN_LEN + 1; + engHk = rawMgmMeasurement + imtq::replySize::RAW_MTM_MEASUREMENT + 1; + calibMgmMeasurement = engHk + imtq::replySize::ENG_HK_DATA_REPLY + 1; + specialRequestReply = calibMgmMeasurement + imtq::replySize::CAL_MTM_MEASUREMENT + 1; + } + uint8_t* rawData; + uint8_t* swReset; + uint8_t* systemState; + uint8_t* startMtmMeasurement; + uint8_t* rawMgmMeasurement; + uint8_t* engHk; + uint8_t* calibMgmMeasurement; + // Share this to reduce amount of copied code for each transfer. + uint8_t* specialRequestReply; +}; + +struct ImtqRepliesWithTorque { + friend class ImtqPollingTask; + + public: + static constexpr size_t BASE_LEN = + imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::ENG_HK_DATA_REPLY + 1 + + imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::RAW_MTM_MEASUREMENT + 1; + ImtqRepliesWithTorque(const uint8_t* rawData) : rawData(const_cast(rawData)) { + initPointers(); + } + + uint8_t* getDipoleActuation() const { return dipoleActuation + 1; } + bool wasDipoleActuationRead() const { return dipoleActuation[0]; } + + uint8_t* getEngHk() const { return engHk + 1; } + bool wasEngHkRead() const { return engHk[0]; }; + + uint8_t* getRawMgmMeasurement() const { return rawMgmMeasurement + 1; } + bool wasGetRawMgmMeasurementRead() const { return rawMgmMeasurement[0]; }; + + private: + void initPointers() { + dipoleActuation = rawData; + engHk = dipoleActuation + imtq::replySize::DEFAULT_MIN_LEN + 1; + startMtmMeasurement = engHk + imtq::replySize::ENG_HK_DATA_REPLY + 1; + rawMgmMeasurement = startMtmMeasurement + imtq::replySize::DEFAULT_MIN_LEN + 1; + } + uint8_t* rawData; + uint8_t* dipoleActuation; + uint8_t* engHk; + uint8_t* startMtmMeasurement; + uint8_t* rawMgmMeasurement; +}; #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_ */ diff --git a/mission/tmtc/CfdpTmFunnel.cpp b/mission/tmtc/CfdpTmFunnel.cpp index e687911f..0ad6512c 100644 --- a/mission/tmtc/CfdpTmFunnel.cpp +++ b/mission/tmtc/CfdpTmFunnel.cpp @@ -53,6 +53,7 @@ ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) { #endif return result; } + size_t packetLen = 0; uint8_t* serPtr = newPacketData; result = spacePacketHeader.serializeBe(&serPtr, &packetLen, spacePacketHeader.getFullPacketLen()); @@ -84,8 +85,8 @@ ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) { sif::error << "PusTmFunnel::handlePacket: Store too full to create data copy or store " "error" << std::endl; - break; #endif + break; } } else { msg.setStorageId(origStoreId); diff --git a/tmtc b/tmtc index 1bc38ea4..5f082cd9 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 1bc38ea4b507ac0e395c52d0b142ad598bb0817c +Subproject commit 5f082cd9fccdaa304f00d92c1f25cdd6a7d8ed0b