diff --git a/mission/devices/MGMHandlerLIS3MDL.cpp b/mission/devices/MGMHandlerLIS3MDL.cpp index 1833ef54..35b71c79 100644 --- a/mission/devices/MGMHandlerLIS3MDL.cpp +++ b/mission/devices/MGMHandlerLIS3MDL.cpp @@ -259,6 +259,7 @@ ReturnValue_t MGMHandlerLIS3MDL::interpretDeviceReply(DeviceCommandId_t id, dataset.fieldStrengthX = mgmX; dataset.fieldStrengthY = mgmY; dataset.fieldStrengthZ = mgmZ; + dataset.setValidity(true, true); dataset.commit(20); } break; @@ -406,10 +407,6 @@ ReturnValue_t MGMHandlerLIS3MDL::prepareCtrlRegisterWrite() { return RETURN_OK; } -void MGMHandlerLIS3MDL::setNormalDatapoolEntriesInvalid() { - // TODO: use new distributed datapools here. -} - void MGMHandlerLIS3MDL::doTransition(Mode_t modeFrom, Submode_t subModeFrom) { } diff --git a/mission/devices/MGMHandlerLIS3MDL.h b/mission/devices/MGMHandlerLIS3MDL.h index b400daee..c48b1fc2 100644 --- a/mission/devices/MGMHandlerLIS3MDL.h +++ b/mission/devices/MGMHandlerLIS3MDL.h @@ -35,28 +35,26 @@ public: protected: /** DeviceHandlerBase overrides */ - virtual void doShutDown() override; - virtual void doStartUp() override; - virtual void doTransition(Mode_t modeFrom, Submode_t subModeFrom) override; - virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; - virtual ReturnValue_t buildCommandFromCommand( + void doShutDown() override; + void doStartUp() override; + void doTransition(Mode_t modeFrom, Submode_t subModeFrom) override; + uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + ReturnValue_t buildCommandFromCommand( DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) override; - virtual ReturnValue_t buildTransitionDeviceCommand( + ReturnValue_t buildTransitionDeviceCommand( DeviceCommandId_t *id) override; - virtual ReturnValue_t buildNormalDeviceCommand( + ReturnValue_t buildNormalDeviceCommand( DeviceCommandId_t *id) override; - virtual ReturnValue_t scanForReply(const uint8_t *start, size_t len, + ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) override; - virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; - virtual void fillCommandAndReplyMap() override; - virtual void modeChanged(void) override; - void setNormalDatapoolEntriesInvalid() override; + void fillCommandAndReplyMap() override; + void modeChanged(void) override; ReturnValue_t initializeLocalDataPool(LocalDataPool &localDataPoolMap, LocalDataPoolManager &poolManager) override; - private: MGMLIS3MDL::MgmPrimaryDataset dataset; diff --git a/mission/devices/MGMHandlerRM3100.cpp b/mission/devices/MGMHandlerRM3100.cpp index 2ff57d85..6040afad 100644 --- a/mission/devices/MGMHandlerRM3100.cpp +++ b/mission/devices/MGMHandlerRM3100.cpp @@ -7,7 +7,11 @@ MGMHandlerRM3100::MGMHandlerRM3100(object_id_t objectId, object_id_t deviceCommunication, CookieIF* comCookie): - DeviceHandlerBase(objectId, deviceCommunication, comCookie) { + DeviceHandlerBase(objectId, deviceCommunication, comCookie), + primaryDataset(this) { +#if OBSW_ENHANCED_PRINTOUT == 1 + debugDivider = new PeriodicOperationDivider(10); +#endif } MGMHandlerRM3100::~MGMHandlerRM3100() {} @@ -140,7 +144,9 @@ ReturnValue_t MGMHandlerRM3100::scanForReply(const uint8_t *start, ReturnValue_t MGMHandlerRM3100::interpretDeviceReply( DeviceCommandId_t id, const uint8_t *packet) { - if(id == RM3100::READ_CMM) { + ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + switch(id) { + case(RM3100::READ_CMM): { if(packet[1] == cmmRegValue) { commandExecuted = true; } @@ -149,13 +155,14 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply( internalState = STATE_CONFIGURE_CMM; return DeviceHandlerIF::DEVICE_REPLY_INVALID; } + break; } - else if(id == RM3100::READ_TMRC) { + case(RM3100::READ_TMRC): { if(packet[1] == tmrcRegValue) { commandExecuted = true; - // Reading TMRC was commanded. - if(mode == MODE_NORMAL) { - + // Reading TMRC was commanded. Trigger event to inform ground. + if(mode != _MODE_START_UP) { + triggerEvent(tmrcSet, tmrcRegValue, 0); } } else { @@ -163,22 +170,33 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply( internalState = STATE_CONFIGURE_TMRC; return DeviceHandlerIF::DEVICE_REPLY_INVALID; } + break; } - else if(id == RM3100::READ_DATA) { - // analyze data here. - // Field strengths in micro Tesla - int32_t fieldStrengthX = (packet[1] << 16 | packet[2] << 8 | packet[3]) - * scaleFactorX; - int32_t fieldStrengthY = (packet[4] << 16 | packet[5] << 8 | packet[6]) - * scaleFactorY; - int32_t fieldStrengthZ = (packet[7] << 16 | packet[8] << 8 | packet[9]) - * scaleFactorZ; + case(RM3100::READ_CYCLE_COUNT): { + uint16_t cycleCountX = packet[1] << 8 | packet[2]; + uint16_t cycleCountY = packet[3] << 8 | packet[4]; + uint16_t cycleCountZ = packet[5] << 8 | packet[6]; + if(cycleCountX != cycleCountRegValueX or + cycleCountY != cycleCountRegValueY or + cycleCountZ != cycleCountRegValueZ) { + return DeviceHandlerIF::DEVICE_REPLY_INVALID; + } + // Reading TMRC was commanded. Trigger event to inform ground. + if(mode != _MODE_START_UP) { + uint32_t eventParam1 = cycleCountX << 16 | cycleCountY; + triggerEvent(cycleCountersSet, eventParam1, cycleCountZ); + } + break; } - else { + case(RM3100::READ_DATA): { + result = handleDataReadout(packet); + break; + } + default: return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; } - return RETURN_OK; + return result; } ReturnValue_t MGMHandlerRM3100::handleCycleCountConfigCommand( @@ -252,3 +270,66 @@ ReturnValue_t MGMHandlerRM3100::handleTmrcConfigCommand( return HasReturnvaluesIF::RETURN_OK; } +void MGMHandlerRM3100::fillCommandAndReplyMap() { + insertInCommandAndReplyMap(RM3100::CONFIGURE_CMM, 1); + insertInCommandAndReplyMap(RM3100::READ_CMM, 1); + + insertInCommandAndReplyMap(RM3100::CONFIGURE_TMRC, 1); + insertInCommandAndReplyMap(RM3100::READ_TMRC, 1); + + insertInCommandAndReplyMap(RM3100::CONFIGURE_CYCLE_COUNT, 1); + insertInCommandAndReplyMap(RM3100::READ_CYCLE_COUNT, 1); + + insertInCommandAndReplyMap(RM3100::READ_DATA, 1, &primaryDataset); +} + +void MGMHandlerRM3100::modeChanged(void) { + internalState = STATE_NONE; +} + +ReturnValue_t MGMHandlerRM3100::initializeLocalDataPool( + LocalDataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_X, + new PoolEntry(0.0)); + localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_Y, + new PoolEntry(0.0)); + localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_Z, + new PoolEntry(0.0)); + return HasReturnvaluesIF::RETURN_OK; +} + +uint32_t MGMHandlerRM3100::getTransitionDelayMs(Mode_t from, Mode_t to) { + return 5000; +} + +ReturnValue_t MGMHandlerRM3100::handleDataReadout(const uint8_t *packet) { + // analyze data here. + // Field strengths in micro Tesla + int32_t fieldStrengthX = (packet[1] << 16 | packet[2] << 8 | packet[3]) + * scaleFactorX; + int32_t fieldStrengthY = (packet[4] << 16 | packet[5] << 8 | packet[6]) + * scaleFactorY; + int32_t fieldStrengthZ = (packet[7] << 16 | packet[8] << 8 | packet[9]) + * scaleFactorZ; + +#if OBSW_ENHANCED_PRINTOUT == 1 + if(debugDivider->checkAndIncrement()) { + sif::info << "MGMHandlerLIS3: Magnetic field strength in" + " microtesla:" << std::endl; + // Set terminal to utf-8 if there is an issue with micro printout. + sif::info << "X: " << fieldStrengthX << " \xC2\xB5T" << std::endl; + sif::info << "Y: " << fieldStrengthY << " \xC2\xB5T" << std::endl; + sif::info << "Z: " << fieldStrengthZ << " \xC2\xB5T" << std::endl; + } +#endif + + ReturnValue_t result = primaryDataset.read(20); + if(result == HasReturnvaluesIF::RETURN_OK) { + primaryDataset.fieldStrengthX = fieldStrengthX; + primaryDataset.fieldStrengthY = fieldStrengthY; + primaryDataset.fieldStrengthZ = fieldStrengthZ; + primaryDataset.setValidity(true, true); + result = primaryDataset.commit(20); + } + return result; +} diff --git a/mission/devices/MGMHandlerRM3100.h b/mission/devices/MGMHandlerRM3100.h index 605ac890..58096a8d 100644 --- a/mission/devices/MGMHandlerRM3100.h +++ b/mission/devices/MGMHandlerRM3100.h @@ -6,10 +6,23 @@ #include +#if OBSW_ENHANCED_PRINTOUT == 1 +#include +#endif + class MGMHandlerRM3100: public DeviceHandlerBase { public: static const uint8_t INTERFACE_ID = CLASS_ID::MGM_RM3100; - static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::MGM_RM3100; + + //! P1: TMRC value which was set, P2: 0 + static constexpr Event tmrcSet = event::makeEvent(SUBSYSTEM_ID::MGM_RM3100, + 0x00, severity::INFO); + + //! P1: First two bytes new Cycle Count X + //! P1: Second two bytes new Cycle Count Y + //! P2: New cycle count Z + static constexpr Event cycleCountersSet = event::makeEvent( + SUBSYSTEM_ID::MGM_RM3100, 0x01, severity::INFO); MGMHandlerRM3100(object_id_t objectId, object_id_t deviceCommunication, CookieIF* comCookie); @@ -18,20 +31,26 @@ public: protected: /* DeviceHandlerBase overrides */ - virtual ReturnValue_t buildTransitionDeviceCommand( + ReturnValue_t buildTransitionDeviceCommand( DeviceCommandId_t *id) override; - virtual void doStartUp() override; - virtual void doShutDown() override; - virtual ReturnValue_t buildNormalDeviceCommand( + void doStartUp() override; + void doShutDown() override; + ReturnValue_t buildNormalDeviceCommand( DeviceCommandId_t *id) override; - virtual ReturnValue_t buildCommandFromCommand( + ReturnValue_t buildCommandFromCommand( DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) override; - virtual ReturnValue_t scanForReply(const uint8_t *start, size_t len, + ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) override; - virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; + void fillCommandAndReplyMap() override; + void modeChanged(void) override; + uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + ReturnValue_t initializeLocalDataPool(LocalDataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; + private: enum InternalState { @@ -48,6 +67,7 @@ private: }; InternalState internalState = InternalState::STATE_NONE; bool commandExecuted = false; + RM3100::Rm3100PrimaryDataset primaryDataset; uint8_t commandBuffer[10]; uint8_t commandBufferLen = 0; @@ -68,6 +88,11 @@ private: ReturnValue_t handleTmrcConfigCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData,size_t commandDataLen); + + ReturnValue_t handleDataReadout(const uint8_t* packet); +#if OBSW_ENHANCED_PRINTOUT == 1 + PeriodicOperationDivider* debugDivider; +#endif }; #endif /* MISSION_DEVICEHANDLING_MGMRM3100HANDLER_H_ */