diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 2f21c534..6dce14c8 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -5,6 +5,8 @@ #include #include #include +#include +#include #include #include "OBSWConfig.h" @@ -352,7 +354,7 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* new SpiCookie(addresses::MGM_0_LIS3, gpioIds::MGM_0_LIS3_CS, mgmLis3::MAX_BUFFER_SIZE, spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); - auto mgmLis3Handler0 = new MgmLIS3MDLHandler( + auto mgmLis3Handler0 = new MgmLis3CustomHandler( objects::MGM_0_LIS3_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::LIS3_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::MGM_0_LIS3_HANDLER); mgmLis3Handler0->setCustomFdir(fdir); @@ -369,8 +371,8 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); auto mgmRm3100Handler1 = - new MgmRM3100Handler(objects::MGM_1_RM3100_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, - spi::RM3100_TRANSITION_DELAY); + new MgmRm3100CustomHandler(objects::MGM_1_RM3100_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, + spi::RM3100_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::MGM_1_RM3100_HANDLER); mgmRm3100Handler1->setCustomFdir(fdir); assemblyChildren[1] = mgmRm3100Handler1; @@ -384,7 +386,7 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* spiCookie = new SpiCookie(addresses::MGM_2_LIS3, gpioIds::MGM_2_LIS3_CS, mgmLis3::MAX_BUFFER_SIZE, spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); - auto* mgmLis3Handler2 = new MgmLIS3MDLHandler( + auto* mgmLis3Handler2 = new MgmLis3CustomHandler( objects::MGM_2_LIS3_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::LIS3_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::MGM_2_LIS3_HANDLER); mgmLis3Handler2->setCustomFdir(fdir); @@ -401,8 +403,8 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); auto* mgmRm3100Handler3 = - new MgmRM3100Handler(objects::MGM_3_RM3100_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, - spi::RM3100_TRANSITION_DELAY); + new MgmRm3100CustomHandler(objects::MGM_3_RM3100_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, + spi::RM3100_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::MGM_3_RM3100_HANDLER); mgmRm3100Handler3->setCustomFdir(fdir); assemblyChildren[3] = mgmRm3100Handler3; diff --git a/mission/core/pollingSeqTables.cpp b/mission/core/pollingSeqTables.cpp index cdf086fd..6823634e 100644 --- a/mission/core/pollingSeqTables.cpp +++ b/mission/core/pollingSeqTables.cpp @@ -491,9 +491,9 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg DeviceHandlerIF::SEND_WRITE); thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ); thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, @@ -505,9 +505,9 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); + length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ); thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); + length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ); } if (enableBside) { // B side @@ -517,9 +517,9 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg DeviceHandlerIF::SEND_WRITE); thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ); thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, @@ -531,9 +531,9 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); + length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ); thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); + length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ); } if (enableAside) { thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, diff --git a/mission/devices/CMakeLists.txt b/mission/devices/CMakeLists.txt index adcbaf73..cb6c066a 100644 --- a/mission/devices/CMakeLists.txt +++ b/mission/devices/CMakeLists.txt @@ -16,6 +16,8 @@ target_sources( RadiationSensorHandler.cpp GyrAdis1650XHandler.cpp GyrL3gCustomHandler.cpp + MgmRm3100CustomHandler.cpp + MgmLis3CustomHandler.cpp RwHandler.cpp max1227.cpp SusHandler.cpp diff --git a/mission/devices/GyrAdis1650XHandler.cpp b/mission/devices/GyrAdis1650XHandler.cpp index 7a13ec1a..fa28bed5 100644 --- a/mission/devices/GyrAdis1650XHandler.cpp +++ b/mission/devices/GyrAdis1650XHandler.cpp @@ -15,6 +15,10 @@ GyrAdis1650XHandler::GyrAdis1650XHandler(object_id_t objectId, object_id_t devic } void GyrAdis1650XHandler::doStartUp() { + if (internalState != InternalState::STARTUP) { + internalState = InternalState::STARTUP; + commandExecuted = false; + } // Initial 310 ms start up time after power-up if (internalState == InternalState::STARTUP) { if (not commandExecuted) { @@ -29,19 +33,9 @@ void GyrAdis1650XHandler::doStartUp() { } else { setMode(MODE_ON); } + internalState = InternalState::NONE; } } - - // // Read all configuration registers first - // if (internalState == InternalState::CONFIG) { - // if (commandExecuted) { - // commandExecuted = false; - // internalState = InternalState::IDLE; - // } - // } - // - // if (internalState == InternalState::IDLE) { - // } } void GyrAdis1650XHandler::doShutDown() { @@ -50,8 +44,10 @@ void GyrAdis1650XHandler::doShutDown() { primaryDataset.setValidity(false, true); internalState = InternalState::SHUTDOWN; } - if (commandExecuted) { + if (internalState == InternalState::SHUTDOWN and commandExecuted) { updatePeriodicReply(false, adis1650x::REPLY); + internalState = InternalState::NONE; + commandExecuted = false; setMode(_MODE_POWER_DOWN); } } diff --git a/mission/devices/GyrAdis1650XHandler.h b/mission/devices/GyrAdis1650XHandler.h index 3d8e82cf..020dcd6e 100644 --- a/mission/devices/GyrAdis1650XHandler.h +++ b/mission/devices/GyrAdis1650XHandler.h @@ -50,7 +50,7 @@ class GyrAdis1650XHandler : public DeviceHandlerBase { bool goToNormalMode = false; bool warningSwitch = true; - enum class InternalState { STARTUP, SHUTDOWN, IDLE }; + enum class InternalState { NONE, STARTUP, SHUTDOWN }; InternalState internalState = InternalState::STARTUP; bool commandExecuted = false; diff --git a/mission/devices/GyrL3gCustomHandler.cpp b/mission/devices/GyrL3gCustomHandler.cpp index f1fb31ab..40de2650 100644 --- a/mission/devices/GyrL3gCustomHandler.cpp +++ b/mission/devices/GyrL3gCustomHandler.cpp @@ -15,7 +15,7 @@ GyrL3gCustomHandler::GyrL3gCustomHandler(object_id_t objectId, object_id_t devic GyrL3gCustomHandler::~GyrL3gCustomHandler() = default; void GyrL3gCustomHandler::doStartUp() { - if (internalState == InternalState::NONE) { + if (internalState != InternalState::STARTUP) { internalState = InternalState::STARTUP; updatePeriodicReply(true, l3gd20h::REPLY); commandExecuted = false; @@ -28,6 +28,8 @@ void GyrL3gCustomHandler::doStartUp() { } else { setMode(_MODE_TO_ON); } + internalState == InternalState::NONE; + commandExecuted = false; } } } @@ -37,7 +39,7 @@ void GyrL3gCustomHandler::doShutDown() { internalState = InternalState::SHUTDOWN; commandExecuted = false; } - if (commandExecuted) { + if (internalState == InternalState::SHUTDOWN and commandExecuted) { internalState = InternalState::NONE; updatePeriodicReply(false, l3gd20h::REPLY); commandExecuted = false; diff --git a/mission/devices/GyrL3gCustomHandler.h b/mission/devices/GyrL3gCustomHandler.h index 7d762ad8..5f840cfc 100644 --- a/mission/devices/GyrL3gCustomHandler.h +++ b/mission/devices/GyrL3gCustomHandler.h @@ -77,7 +77,6 @@ class GyrL3gCustomHandler : public DeviceHandlerBase { uint8_t ctrlReg4Value = l3gd20h::CTRL_REG_4_VAL; uint8_t ctrlReg5Value = l3gd20h::CTRL_REG_5_VAL; - std::array cmdBuf; acs::GyroL3gRequest gyrRequest{}; // Set default value diff --git a/mission/devices/MgmLis3CustomHandler.cpp b/mission/devices/MgmLis3CustomHandler.cpp new file mode 100644 index 00000000..5c94266a --- /dev/null +++ b/mission/devices/MgmLis3CustomHandler.cpp @@ -0,0 +1,146 @@ +#include + +#include + +#include "fsfw/datapool/PoolReadGuard.h" + +MgmLis3CustomHandler::MgmLis3CustomHandler(object_id_t objectId, object_id_t deviceCommunication, + CookieIF *comCookie, uint32_t transitionDelay) + : DeviceHandlerBase(objectId, deviceCommunication, comCookie), + dataset(this), + transitionDelay(transitionDelay) {} + +MgmLis3CustomHandler::~MgmLis3CustomHandler() = default; + +void MgmLis3CustomHandler::doStartUp() { + if (internalState != InternalState::STARTUP) { + commandExecuted = false; + updatePeriodicReply(true, REPLY); + internalState = InternalState::STARTUP; + } + if (internalState == InternalState::STARTUP) { + if (commandExecuted) { + setMode(MODE_NORMAL); + internalState = InternalState::NONE; + commandExecuted = false; + } + } +} + +void MgmLis3CustomHandler::doShutDown() { + if (internalState != InternalState::SHUTDOWN) { + internalState = InternalState::SHUTDOWN; + commandExecuted = false; + } + if (internalState == InternalState::SHUTDOWN and commandExecuted) { + updatePeriodicReply(false, REPLY); + commandExecuted = false; + internalState = InternalState::NONE; + setMode(_MODE_POWER_DOWN); + } +} + +ReturnValue_t MgmLis3CustomHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + if (internalState == InternalState::STARTUP) { + *id = REQUEST; + return prepareRequest(acs::SimpleSensorMode::NORMAL); + } else if (internalState == InternalState::SHUTDOWN) { + *id = REQUEST; + return prepareRequest(acs::SimpleSensorMode::OFF); + } + return NOTHING_TO_SEND; +} + +ReturnValue_t MgmLis3CustomHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { + *id = REQUEST; + return prepareRequest(acs::SimpleSensorMode::NORMAL); +} + +ReturnValue_t MgmLis3CustomHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, + size_t commandDataLen) { + return NOTHING_TO_SEND; +} + +ReturnValue_t MgmLis3CustomHandler::scanForReply(const uint8_t *start, size_t len, + DeviceCommandId_t *foundId, size_t *foundLen) { + if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) { + return IGNORE_FULL_PACKET; + } + if (len != sizeof(acs::MgmRm3100Reply)) { + *foundLen = len; + return returnvalue::FAILED; + } + *foundId = REPLY; + *foundLen = len; + return returnvalue::OK; +} +ReturnValue_t MgmLis3CustomHandler::interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) { + const auto *reply = reinterpret_cast(packet); + if (reply->dataWasSet) { + if (internalState == InternalState::STARTUP) { + commandExecuted = true; + } + PoolReadGuard pg(&dataset); + for (uint8_t idx = 0; idx < 3; idx++) { + dataset.fieldStrengths[idx] = + reply->mgmValuesRaw[idx] * reply->sensitivity * mgmLis3::GAUSS_TO_MICROTESLA_FACTOR; + } + + dataset.setValidity(true, true); + if (std::abs(dataset.fieldStrengths[0]) > absLimitX or + std::abs(dataset.fieldStrengths[1]) > absLimitY or + std::abs(dataset.fieldStrengths[2]) > absLimitZ) { + dataset.fieldStrengths.setValid(false); + } + dataset.temperature = 25.0 + ((static_cast(reply->temperatureRaw)) / 8.0); + } + return returnvalue::OK; +} + +void MgmLis3CustomHandler::fillCommandAndReplyMap() { + insertInCommandMap(REQUEST); + insertInReplyMap(REPLY, 5, nullptr, 0, true); +} + +void MgmLis3CustomHandler::setToGoToNormalMode(bool enable) { this->goToNormalMode = enable; } + +uint32_t MgmLis3CustomHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { + return transitionDelay; +} + +void MgmLis3CustomHandler::modeChanged(void) { internalState = InternalState::NONE; } + +ReturnValue_t MgmLis3CustomHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(mgmLis3::FIELD_STRENGTHS, &mgmXYZ); + localDataPoolMap.emplace(mgmLis3::TEMPERATURE_CELCIUS, &temperature); + poolManager.subscribeForRegularPeriodicPacket({dataset.getSid(), false, 10.0}); + return returnvalue::OK; +} + +void MgmLis3CustomHandler::setAbsoluteLimits(float xLimit, float yLimit, float zLimit) { + this->absLimitX = xLimit; + this->absLimitY = yLimit; + this->absLimitZ = zLimit; +} + +void MgmLis3CustomHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) { + periodicPrintout = enable; + debugDivider.setDivider(divider); +} + +ReturnValue_t MgmLis3CustomHandler::prepareRequest(acs::SimpleSensorMode mode) { + request.mode = mode; + rawPacket = reinterpret_cast(&request); + rawPacketLen = sizeof(acs::MgmLis3Request); + return returnvalue::OK; +} + +LocalPoolDataSetBase *MgmLis3CustomHandler::getDataSetHandle(sid_t sid) { + if (sid == dataset.getSid()) { + return &dataset; + } + return nullptr; +} diff --git a/mission/devices/MgmLis3CustomHandler.h b/mission/devices/MgmLis3CustomHandler.h new file mode 100644 index 00000000..58e7e182 --- /dev/null +++ b/mission/devices/MgmLis3CustomHandler.h @@ -0,0 +1,105 @@ +#ifndef MISSION_DEVICES_MGMLIS3CUSTOMHANDLER_H_ +#define MISSION_DEVICES_MGMLIS3CUSTOMHANDLER_H_ + +#include + +#include "fsfw/devicehandlers/DeviceHandlerBase.h" +#include "fsfw/globalfunctions/PeriodicOperationDivider.h" +#include "mission/devices/devicedefinitions/acsPolling.h" + +class PeriodicOperationDivider; + +/** + * @brief Device handler object for the LIS3MDL 3-axis magnetometer + * by STMicroeletronics + * @details + * Datasheet can be found online by googling LIS3MDL. + * Flight manual: + * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/LIS3MDL_MGM + * @author L. Loidold, R. Mueller + */ +class MgmLis3CustomHandler : public DeviceHandlerBase { + public: + static constexpr DeviceCommandId_t REQUEST = 0x70; + static constexpr DeviceCommandId_t REPLY = 0x77; + + static const uint8_t INTERFACE_ID = CLASS_ID::MGM_LIS3MDL; + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::MGM_LIS3MDL; + // Notifies a command to change the setup parameters + static const Event CHANGE_OF_SETUP_PARAMETER = MAKE_EVENT(0, severity::LOW); + + MgmLis3CustomHandler(uint32_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, + uint32_t transitionDelay); + virtual ~MgmLis3CustomHandler(); + + void enablePeriodicPrintouts(bool enable, uint8_t divider); + /** + * Set the absolute limit for the values on the axis in microtesla. The dataset values will + * be marked as invalid if that limit is exceeded + * @param xLimit + * @param yLimit + * @param zLimit + */ + void setAbsoluteLimits(float xLimit, float yLimit, float zLimit); + void setToGoToNormalMode(bool enable); + + protected: + /** DeviceHandlerBase overrides */ + void doShutDown() override; + void doStartUp() override; + virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) override; + ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, + size_t *foundLen) override; + /** + * This implementation is tailored towards space applications and will flag values larger + * than 100 microtesla on X,Y and 150 microtesla on Z as invalid + * @param id + * @param packet + * @return + */ + virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; + void fillCommandAndReplyMap() override; + void modeChanged(void) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; + LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override; + + private: + mgmLis3::MgmPrimaryDataset dataset; + acs::MgmLis3Request request; + + uint32_t transitionDelay; + + float absLimitX = 100; + float absLimitY = 100; + float absLimitZ = 150; + + uint8_t statusRegister = 0; + bool goToNormalMode = false; + + enum class InternalState { + NONE, + STARTUP, + SHUTDOWN, + }; + + InternalState internalState = InternalState::NONE; + bool commandExecuted = false; + + PoolEntry mgmXYZ = PoolEntry(3); + PoolEntry temperature = PoolEntry(); + /*------------------------------------------------------------------------*/ + /* Device specific commands and variables */ + /*------------------------------------------------------------------------*/ + + bool periodicPrintout = false; + PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3); + + ReturnValue_t prepareRequest(acs::SimpleSensorMode mode); +}; + +#endif /* MISSION_DEVICES_MGMLIS3CUSTOMHANDLER_H_ */ diff --git a/mission/devices/MgmRm3100CustomHandler.cpp b/mission/devices/MgmRm3100CustomHandler.cpp new file mode 100644 index 00000000..4c40bf29 --- /dev/null +++ b/mission/devices/MgmRm3100CustomHandler.cpp @@ -0,0 +1,127 @@ +#include + +#include "fsfw/datapool/PoolReadGuard.h" +#include "fsfw/devicehandlers/DeviceHandlerMessage.h" +#include "fsfw/globalfunctions/bitutility.h" +#include "fsfw/objectmanager/SystemObjectIF.h" +#include "fsfw/returnvalues/returnvalue.h" + +MgmRm3100CustomHandler::MgmRm3100CustomHandler(object_id_t objectId, + object_id_t deviceCommunication, CookieIF *comCookie, + uint32_t transitionDelay) + : DeviceHandlerBase(objectId, deviceCommunication, comCookie), + primaryDataset(this), + transitionDelay(transitionDelay) {} + +MgmRm3100CustomHandler::~MgmRm3100CustomHandler() = default; + +void MgmRm3100CustomHandler::doStartUp() { + if (internalState != InternalState::STARTUP) { + commandExecuted = false; + updatePeriodicReply(true, REPLY); + internalState = InternalState::STARTUP; + } + if (internalState == InternalState::STARTUP) { + if (commandExecuted) { + commandExecuted = false; + internalState = InternalState::NONE; + setMode(MODE_NORMAL); + } + } +} + +void MgmRm3100CustomHandler::doShutDown() { + if (internalState != InternalState::SHUTDOWN) { + commandExecuted = false; + internalState = InternalState::SHUTDOWN; + } + if (internalState == InternalState::SHUTDOWN and commandExecuted) { + updatePeriodicReply(false, REPLY); + setMode(_MODE_POWER_DOWN); + commandExecuted = false; + } +} + +ReturnValue_t MgmRm3100CustomHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + if (internalState == InternalState::STARTUP) { + *id = REQUEST; + return prepareRequest(acs::SimpleSensorMode::NORMAL); + } else if (internalState == InternalState::SHUTDOWN) { + *id = REQUEST; + return prepareRequest(acs::SimpleSensorMode::OFF); + } + return NOTHING_TO_SEND; +} + +ReturnValue_t MgmRm3100CustomHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, + size_t commandDataLen) { + return NOTHING_TO_SEND; +} + +ReturnValue_t MgmRm3100CustomHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { + *id = REQUEST; + return prepareRequest(acs::SimpleSensorMode::NORMAL); +} + +ReturnValue_t MgmRm3100CustomHandler::scanForReply(const uint8_t *start, size_t len, + DeviceCommandId_t *foundId, size_t *foundLen) { + if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) { + return IGNORE_FULL_PACKET; + } + if (len != sizeof(acs::MgmRm3100Reply)) { + *foundLen = len; + return returnvalue::FAILED; + } + *foundId = REPLY; + *foundLen = len; + return returnvalue::OK; +} + +ReturnValue_t MgmRm3100CustomHandler::interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) { + const auto *reply = reinterpret_cast(packet); + if (reply->dataWasRead) { + if (internalState == InternalState::STARTUP) { + commandExecuted = true; + } + + PoolReadGuard pg(&primaryDataset); + for (uint8_t idx = 0; idx < 3; idx++) { + primaryDataset.fieldStrengths[idx] = reply->mgmValuesRaw[idx] * reply->scaleFactors[idx]; + } + } + return returnvalue::OK; +} + +void MgmRm3100CustomHandler::fillCommandAndReplyMap() { + insertInCommandMap(REQUEST); + insertInReplyMap(REPLY, 5, nullptr, 0, true); +} + +void MgmRm3100CustomHandler::modeChanged() { internalState = InternalState::NONE; } + +ReturnValue_t MgmRm3100CustomHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(mgmRm3100::FIELD_STRENGTHS, &mgmXYZ); + poolManager.subscribeForRegularPeriodicPacket({primaryDataset.getSid(), false, 10.0}); + return returnvalue::OK; +} + +uint32_t MgmRm3100CustomHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { + return this->transitionDelay; +} + +void MgmRm3100CustomHandler::setToGoToNormalMode(bool enable) { goToNormalModeAtStartup = enable; } + +void MgmRm3100CustomHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) { + periodicPrintout = enable; + debugDivider.setDivider(divider); +} + +ReturnValue_t MgmRm3100CustomHandler::prepareRequest(acs::SimpleSensorMode mode) { + request.mode = mode; + rawPacket = reinterpret_cast(&request); + rawPacketLen = sizeof(acs::MgmRm3100Request); + return returnvalue::OK; +} diff --git a/mission/devices/MgmRm3100CustomHandler.h b/mission/devices/MgmRm3100CustomHandler.h new file mode 100644 index 00000000..c4821cc7 --- /dev/null +++ b/mission/devices/MgmRm3100CustomHandler.h @@ -0,0 +1,98 @@ +#ifndef MISSION_DEVICES_MGMRM3100CUSTOMHANDLER_H_ +#define MISSION_DEVICES_MGMRM3100CUSTOMHANDLER_H_ + +#include + +#include "fsfw/devicehandlers/DeviceHandlerBase.h" +#include "fsfw/globalfunctions/PeriodicOperationDivider.h" +#include "mission/devices/devicedefinitions/acsPolling.h" + +/** + * @brief Device Handler for the RM3100 geomagnetic magnetometer sensor + * (https://www.pnicorp.com/rm3100/) + * @details + * Flight manual: + * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/RM3100_MGM + */ +class MgmRm3100CustomHandler : public DeviceHandlerBase { + public: + static constexpr DeviceCommandId_t REQUEST = 0x70; + static constexpr DeviceCommandId_t REPLY = 0x77; + + static const uint8_t INTERFACE_ID = CLASS_ID::MGM_RM3100; + + //! [EXPORT] : [COMMENT] P1: TMRC value which was set, P2: 0 + static constexpr Event TMRC_SET = + event::makeEvent(SUBSYSTEM_ID::MGM_RM3100, 0x00, severity::INFO); + + //! [EXPORT] : [COMMENT] Cycle counter set. 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); + + MgmRm3100CustomHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, + uint32_t transitionDelay); + virtual ~MgmRm3100CustomHandler(); + + void enablePeriodicPrintouts(bool enable, uint8_t divider); + /** + * Configure device handler to go to normal mode after startup immediately + * @param enable + */ + void setToGoToNormalMode(bool enable); + + protected: + /* DeviceHandlerBase overrides */ + ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override; + void doStartUp() override; + void doShutDown() override; + ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) override; + ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, + size_t *foundLen) override; + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; + + void fillCommandAndReplyMap() override; + void modeChanged(void) override; + virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; + + private: + enum class InternalState { NONE, STARTUP, SHUTDOWN }; + InternalState internalState = InternalState::NONE; + bool commandExecuted = false; + mgmRm3100::Rm3100PrimaryDataset primaryDataset; + acs::MgmRm3100Request request{}; + + // uint8_t cmmRegValue = mgmRm3100::CMM_VALUE; + // uint8_t tmrcRegValue = mgmRm3100::TMRC_DEFAULT_VALUE; + // uint16_t cycleCountRegValueX = mgmRm3100::CYCLE_COUNT_VALUE; + // uint16_t cycleCountRegValueY = mgmRm3100::CYCLE_COUNT_VALUE; + // uint16_t cycleCountRegValueZ = mgmRm3100::CYCLE_COUNT_VALUE; + float scaleFactorX = 1.0 / mgmRm3100::DEFAULT_GAIN; + float scaleFactorY = 1.0 / mgmRm3100::DEFAULT_GAIN; + float scaleFactorZ = 1.0 / mgmRm3100::DEFAULT_GAIN; + + bool goToNormalModeAtStartup = false; + uint32_t transitionDelay; + PoolEntry mgmXYZ = PoolEntry(3); + + ReturnValue_t handleCycleCountConfigCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, size_t commandDataLen); + ReturnValue_t handleCycleCommand(bool oneCycleValue, const uint8_t *commandData, + size_t commandDataLen); + + ReturnValue_t handleTmrcConfigCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen); + + // ReturnValue_t handleDataReadout(const uint8_t *packet); + + bool periodicPrintout = false; + ReturnValue_t prepareRequest(acs::SimpleSensorMode mode); + PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3); +}; + +#endif /* MISSION_DEVICEHANDLING_MgmRm3100CustomHandler_H_ */