diff --git a/CHANGELOG.md b/CHANGELOG.md index ae5a7323..ce15872b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -27,6 +27,7 @@ will consitute of a breaking change warranting a new major release: debugging images 2 minutes to start the watchdog without the watch functionality. - The SD card prefix is now set earlier inside the `CoreController` constructor - The watchdog handling was moved outside the `CoreController` into the main loop. +- Move ACS board polling to separate worker thread. ## Fixed diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 41098175..c8eb4cc9 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -1,8 +1,12 @@ #include "ObjectFactory.h" #include +#include #include #include +#include +#include +#include #include #include "OBSWConfig.h" @@ -59,6 +63,7 @@ #if OBSW_TEST_LIBGPIOD == 1 #include "linux/boardtest/LibgpiodTest.h" #endif +#include #include #include #include @@ -85,7 +90,6 @@ #include "mission/core/GenericFactory.h" #include "mission/devices/ACUHandler.h" #include "mission/devices/BpxBatteryHandler.h" -#include "mission/devices/GyroADIS1650XHandler.h" #include "mission/devices/HeaterHandler.h" #include "mission/devices/Max31865PT1000Handler.h" #include "mission/devices/P60DockHandler.h" @@ -241,8 +245,8 @@ ReturnValue_t ObjectFactory::createRadSensorComponent(LinuxLibgpioIF* gpioComIF, return returnvalue::OK; } -void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF, - PowerSwitchIF& pwrSwitcher) { +void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* gpioComIF, + SerialComIF* uartComIF, PowerSwitchIF& pwrSwitcher) { using namespace gpio; GpioCookie* gpioCookieAcsBoard = new GpioCookie(); @@ -345,14 +349,16 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo static_cast(fdir); #if OBSW_ADD_ACS_BOARD == 1 + new AcsBoardPolling(objects::ACS_BOARD_POLLING_TASK, spiComIF, *gpioComIF); std::string spiDev = q7s::SPI_DEFAULT_DEV; std::array assemblyChildren; SpiCookie* spiCookie = - new SpiCookie(addresses::MGM_0_LIS3, gpioIds::MGM_0_LIS3_CS, MGMLIS3MDL::MAX_BUFFER_SIZE, + 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( - objects::MGM_0_LIS3_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::LIS3_TRANSITION_DELAY); + auto mgmLis3Handler0 = + new MgmLis3CustomHandler(objects::MGM_0_LIS3_HANDLER, objects::ACS_BOARD_POLLING_TASK, + spiCookie, spi::LIS3_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::MGM_0_LIS3_HANDLER); mgmLis3Handler0->setCustomFdir(fdir); assemblyChildren[0] = mgmLis3Handler0; @@ -364,12 +370,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo mgmLis3Handler->enablePeriodicPrintouts(true, 10); #endif spiCookie = - new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, RM3100::MAX_BUFFER_SIZE, + new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, mgmRm3100::MAX_BUFFER_SIZE, 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::ACS_BOARD_POLLING_TASK, + spiCookie, spi::RM3100_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::MGM_1_RM3100_HANDLER); mgmRm3100Handler1->setCustomFdir(fdir); assemblyChildren[1] = mgmRm3100Handler1; @@ -380,12 +386,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo #if OBSW_DEBUG_ACS == 1 mgmRm3100Handler->enablePeriodicPrintouts(true, 10); #endif - spiCookie = - new SpiCookie(addresses::MGM_2_LIS3, gpioIds::MGM_2_LIS3_CS, MGMLIS3MDL::MAX_BUFFER_SIZE, - spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED); + 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( - objects::MGM_2_LIS3_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::LIS3_TRANSITION_DELAY); + auto* mgmLis3Handler2 = + new MgmLis3CustomHandler(objects::MGM_2_LIS3_HANDLER, objects::ACS_BOARD_POLLING_TASK, + spiCookie, spi::LIS3_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::MGM_2_LIS3_HANDLER); mgmLis3Handler2->setCustomFdir(fdir); assemblyChildren[2] = mgmLis3Handler2; @@ -397,12 +403,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo mgmLis3Handler->enablePeriodicPrintouts(true, 10); #endif spiCookie = - new SpiCookie(addresses::MGM_3_RM3100, gpioIds::MGM_3_RM3100_CS, RM3100::MAX_BUFFER_SIZE, + new SpiCookie(addresses::MGM_3_RM3100, gpioIds::MGM_3_RM3100_CS, mgmRm3100::MAX_BUFFER_SIZE, 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::ACS_BOARD_POLLING_TASK, + spiCookie, spi::RM3100_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::MGM_3_RM3100_HANDLER); mgmRm3100Handler3->setCustomFdir(fdir); assemblyChildren[3] = mgmRm3100Handler3; @@ -416,12 +422,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo // Commented until ACS board V2 in in clean room again // Gyro 0 Side A spiCookie = - new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, ADIS1650X::MAXIMUM_REPLY_SIZE, + new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, adis1650x::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); auto adisHandler = - new GyroADIS1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, - ADIS1650X::Type::ADIS16505); + new GyrAdis1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::ACS_BOARD_POLLING_TASK, + spiCookie, adis1650x::Type::ADIS16505); fdir = new AcsBoardFdir(objects::GYRO_0_ADIS_HANDLER); adisHandler->setCustomFdir(fdir); assemblyChildren[4] = adisHandler; @@ -433,11 +439,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo adisHandler->enablePeriodicPrintouts(true, 10); #endif // Gyro 1 Side A - spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, L3GD20H::MAX_BUFFER_SIZE, + spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, l3gd20h::MAX_BUFFER_SIZE, spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); - auto gyroL3gHandler1 = new GyroHandlerL3GD20H( - objects::GYRO_1_L3G_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::L3G_TRANSITION_DELAY); + auto gyroL3gHandler1 = + new GyrL3gCustomHandler(objects::GYRO_1_L3G_HANDLER, objects::ACS_BOARD_POLLING_TASK, + spiCookie, spi::L3G_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::GYRO_1_L3G_HANDLER); gyroL3gHandler1->setCustomFdir(fdir); assemblyChildren[5] = gyroL3gHandler1; @@ -450,11 +457,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo #endif // Gyro 2 Side B spiCookie = - new SpiCookie(addresses::GYRO_2_ADIS, gpioIds::GYRO_2_ADIS_CS, ADIS1650X::MAXIMUM_REPLY_SIZE, + new SpiCookie(addresses::GYRO_2_ADIS, gpioIds::GYRO_2_ADIS_CS, adis1650x::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); - adisHandler = new GyroADIS1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::SPI_MAIN_COM_IF, - spiCookie, ADIS1650X::Type::ADIS16505); + adisHandler = + new GyrAdis1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::ACS_BOARD_POLLING_TASK, + spiCookie, adis1650x::Type::ADIS16505); fdir = new AcsBoardFdir(objects::GYRO_2_ADIS_HANDLER); adisHandler->setCustomFdir(fdir); assemblyChildren[6] = adisHandler; @@ -463,11 +471,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo adisHandler->setToGoToNormalModeImmediately(); #endif // Gyro 3 Side B - spiCookie = new SpiCookie(addresses::GYRO_3_L3G, gpioIds::GYRO_3_L3G_CS, L3GD20H::MAX_BUFFER_SIZE, + spiCookie = new SpiCookie(addresses::GYRO_3_L3G, gpioIds::GYRO_3_L3G_CS, l3gd20h::MAX_BUFFER_SIZE, spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); - auto gyroL3gHandler3 = new GyroHandlerL3GD20H( - objects::GYRO_3_L3G_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::L3G_TRANSITION_DELAY); + auto gyroL3gHandler3 = + new GyrL3gCustomHandler(objects::GYRO_3_L3G_HANDLER, objects::ACS_BOARD_POLLING_TASK, + spiCookie, spi::L3G_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::GYRO_3_L3G_HANDLER); gyroL3gHandler3->setCustomFdir(fdir); assemblyChildren[7] = gyroL3gHandler3; diff --git a/bsp_q7s/core/ObjectFactory.h b/bsp_q7s/core/ObjectFactory.h index 2118e3d7..7ec6c871 100644 --- a/bsp_q7s/core/ObjectFactory.h +++ b/bsp_q7s/core/ObjectFactory.h @@ -2,6 +2,7 @@ #define BSP_Q7S_OBJECTFACTORY_H_ #include +#include #include #include #include @@ -31,7 +32,7 @@ void createPlPcduComponents(LinuxLibgpioIF* gpioComIF, SpiComIF* spiComIF, PowerSwitchIF* pwrSwitcher, Stack5VHandler& stackHandler); void createTmpComponents(); ReturnValue_t createRadSensorComponent(LinuxLibgpioIF* gpioComIF, Stack5VHandler& handler); -void createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF, +void createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF, PowerSwitchIF& pwrSwitcher); void createHeaterComponents(GpioIF* gpioIF, PowerSwitchIF* pwrSwitcher, HealthTableIF* healthTable, HeaterHandler*& heaterHandler); diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index 7647af8b..7bb2ceec 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -194,6 +194,15 @@ void scheduling::initTasks() { scheduling::printAddObjectError("GPS_CTRL", objects::GPS_CONTROLLER); } +#if OBSW_ADD_ACS_BOARD == 1 + PeriodicTaskIF* acsBrdPolling = factory->createPeriodicTask( + "ACS_BOARD_POLLING", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); + result = acsBrdPolling->addComponent(objects::ACS_BOARD_POLLING_TASK); + if (result != returnvalue::OK) { + scheduling::printAddObjectError("ACS_BOARD_POLLING", objects::ACS_BOARD_POLLING_TASK); + } +#endif + #if OBSW_ADD_RW == 1 PeriodicTaskIF* rwPolling = factory->createPeriodicTask( "RW_POLLING_TASK", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); @@ -351,6 +360,9 @@ void scheduling::initTasks() { #if OBSW_ADD_SA_DEPL == 1 solarArrayDeplTask->startTask(); #endif +#if OBSW_ADD_ACS_BOARD == 1 + acsBrdPolling->startTask(); +#endif #if OBSW_ADD_MGT == 1 imtqPolling->startTask(); #endif diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index 36e9df64..e05e8098 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -43,7 +43,7 @@ void ObjectFactory::produce(void* args) { #endif #if OBSW_ADD_ACS_BOARD == 1 - createAcsBoardComponents(gpioComIF, uartComIF, *pwrSwitcher); + createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher); #endif HeaterHandler* heaterHandler; createHeaterComponents(gpioComIF, pwrSwitcher, healthTable, heaterHandler); diff --git a/common/config/eive/objects.h b/common/config/eive/objects.h index 08f07d3d..c7c7cce9 100644 --- a/common/config/eive/objects.h +++ b/common/config/eive/objects.h @@ -123,7 +123,9 @@ enum commonObjects : uint32_t { // CCSDS_IP_CORE_BRIDGE = 0x73500000, /* 0x49 ('I') for Communication Interfaces */ - SPI_RTD_COM_IF = 0x49020006, + ACS_BOARD_POLLING_TASK = 0x49060004, + RW_POLLING_TASK = 0x49060005, + SPI_RTD_COM_IF = 0x49060006, // 0x60 for other stuff HEATER_0_PLOC_PROC_BRD = 0x60000000, diff --git a/dummies/GyroAdisDummy.cpp b/dummies/GyroAdisDummy.cpp index 050c838d..ed581138 100644 --- a/dummies/GyroAdisDummy.cpp +++ b/dummies/GyroAdisDummy.cpp @@ -1,6 +1,6 @@ #include "GyroAdisDummy.h" -#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h" +#include GyroAdisDummy::GyroAdisDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) : DeviceHandlerBase(objectId, comif, comCookie), dataset(this) {} @@ -40,13 +40,13 @@ uint32_t GyroAdisDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { r ReturnValue_t GyroAdisDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_X, new PoolEntry({-0.5}, true)); - localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Y, new PoolEntry({0.2}, true)); - localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Z, new PoolEntry({-1.2}, true)); - localDataPoolMap.emplace(ADIS1650X::ACCELERATION_X, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Y, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Z, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::TEMPERATURE, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ANG_VELOC_X, new PoolEntry({-0.5}, true)); + localDataPoolMap.emplace(adis1650x::ANG_VELOC_Y, new PoolEntry({0.2}, true)); + localDataPoolMap.emplace(adis1650x::ANG_VELOC_Z, new PoolEntry({-1.2}, true)); + localDataPoolMap.emplace(adis1650x::ACCELERATION_X, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ACCELERATION_Y, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ACCELERATION_Z, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::TEMPERATURE, new PoolEntry({0.0})); return returnvalue::OK; } diff --git a/dummies/GyroAdisDummy.h b/dummies/GyroAdisDummy.h index b3aad620..21de1eeb 100644 --- a/dummies/GyroAdisDummy.h +++ b/dummies/GyroAdisDummy.h @@ -2,8 +2,7 @@ #define DUMMIES_GYROADISDUMMY_H_ #include - -#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h" +#include class GyroAdisDummy : public DeviceHandlerBase { public: diff --git a/dummies/GyroL3GD20Dummy.cpp b/dummies/GyroL3GD20Dummy.cpp index 20309c63..8c89f59d 100644 --- a/dummies/GyroL3GD20Dummy.cpp +++ b/dummies/GyroL3GD20Dummy.cpp @@ -1,6 +1,6 @@ #include "GyroL3GD20Dummy.h" -#include "fsfw_hal/devicehandlers/devicedefinitions/GyroL3GD20Definitions.h" +#include GyroL3GD20Dummy::GyroL3GD20Dummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) : DeviceHandlerBase(objectId, comif, comCookie) {} @@ -40,9 +40,9 @@ uint32_t GyroL3GD20Dummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { ReturnValue_t GyroL3GD20Dummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, new PoolEntry({1.2}, true)); - localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y, new PoolEntry({-0.1}, true)); - localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z, new PoolEntry({0.7}, true)); - localDataPoolMap.emplace(L3GD20H::TEMPERATURE, new PoolEntry({0.0})); + localDataPoolMap.emplace(l3gd20h::ANG_VELOC_X, new PoolEntry({1.2}, true)); + localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Y, new PoolEntry({-0.1}, true)); + localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Z, new PoolEntry({0.7}, true)); + localDataPoolMap.emplace(l3gd20h::TEMPERATURE, new PoolEntry({0.0})); return returnvalue::OK; } diff --git a/dummies/MgmLIS3MDLDummy.cpp b/dummies/MgmLIS3MDLDummy.cpp index 31228260..9038d963 100644 --- a/dummies/MgmLIS3MDLDummy.cpp +++ b/dummies/MgmLIS3MDLDummy.cpp @@ -1,6 +1,6 @@ #include "MgmLIS3MDLDummy.h" -#include "fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h" +#include MgmLIS3MDLDummy::MgmLIS3MDLDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) : DeviceHandlerBase(objectId, comif, comCookie), dataset(this) {} @@ -40,8 +40,8 @@ uint32_t MgmLIS3MDLDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { ReturnValue_t MgmLIS3MDLDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(MGMLIS3MDL::TEMPERATURE_CELCIUS, new PoolEntry({0.0})); - localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTHS, + localDataPoolMap.emplace(mgmLis3::TEMPERATURE_CELCIUS, new PoolEntry({0.0})); + localDataPoolMap.emplace(mgmLis3::FIELD_STRENGTHS, new PoolEntry({1.02, 0.56, -0.78}, true)); return returnvalue::OK; } diff --git a/dummies/MgmLIS3MDLDummy.h b/dummies/MgmLIS3MDLDummy.h index f63ed578..214f2f92 100644 --- a/dummies/MgmLIS3MDLDummy.h +++ b/dummies/MgmLIS3MDLDummy.h @@ -2,8 +2,7 @@ #define DUMMIES_MGMLIS3MDLDUMMY_H_ #include - -#include "fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h" +#include class MgmLIS3MDLDummy : public DeviceHandlerBase { public: @@ -17,7 +16,7 @@ class MgmLIS3MDLDummy : public DeviceHandlerBase { virtual ~MgmLIS3MDLDummy(); protected: - MGMLIS3MDL::MgmPrimaryDataset dataset; + mgmLis3::MgmPrimaryDataset dataset; void doStartUp() override; void doShutDown() override; ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; diff --git a/dummies/MgmRm3100Dummy.cpp b/dummies/MgmRm3100Dummy.cpp index 1bd4f513..fa5582c1 100644 --- a/dummies/MgmRm3100Dummy.cpp +++ b/dummies/MgmRm3100Dummy.cpp @@ -36,7 +36,7 @@ uint32_t MgmRm3100Dummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { ReturnValue_t MgmRm3100Dummy::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { - localDataPoolMap.emplace(RM3100::FIELD_STRENGTHS, + localDataPoolMap.emplace(mgmRm3100::FIELD_STRENGTHS, new PoolEntry({0.87, -0.95, 0.11}, true)); return returnvalue::OK; } diff --git a/dummies/MgmRm3100Dummy.h b/dummies/MgmRm3100Dummy.h index 857e6f69..0fa1004e 100644 --- a/dummies/MgmRm3100Dummy.h +++ b/dummies/MgmRm3100Dummy.h @@ -1,8 +1,9 @@ #ifndef DUMMIES_MGMRM3100DUMMY_H_ #define DUMMIES_MGMRM3100DUMMY_H_ +#include + #include "fsfw/devicehandlers/DeviceHandlerBase.h" -#include "fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h" class MgmRm3100Dummy : public DeviceHandlerBase { public: @@ -10,7 +11,7 @@ class MgmRm3100Dummy : public DeviceHandlerBase { virtual ~MgmRm3100Dummy(); protected: - RM3100::Rm3100PrimaryDataset dataset; + mgmRm3100::Rm3100PrimaryDataset dataset; void doStartUp() override; void doShutDown() override; ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; diff --git a/linux/devices/AcsBoardPolling.cpp b/linux/devices/AcsBoardPolling.cpp new file mode 100644 index 00000000..871cfdd0 --- /dev/null +++ b/linux/devices/AcsBoardPolling.cpp @@ -0,0 +1,730 @@ +#include "AcsBoardPolling.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "devices/gpioIds.h" + +using namespace returnvalue; + +AcsBoardPolling::AcsBoardPolling(object_id_t objectId, SpiComIF& lowLevelComIF, GpioIF& gpioIF) + : SystemObject(objectId), spiComIF(lowLevelComIF), gpioIF(gpioIF) { + semaphore = SemaphoreFactory::instance()->createBinarySemaphore(); + semaphore->acquire(); + ipcLock = MutexFactory::instance()->createMutex(); +} + +ReturnValue_t AcsBoardPolling::performOperation(uint8_t operationCode) { + while (true) { + ipcLock->lockMutex(); + state = InternalState::IDLE; + ipcLock->unlockMutex(); + semaphore->acquire(); + // Give all tasks or the PST some time to submit all consecutive requests. + TaskFactory::delayTask(2); + gyroAdisHandler(gyro0Adis); + gyroAdisHandler(gyro2Adis); + gyroL3gHandler(gyro1L3g); + gyroL3gHandler(gyro3L3g); + mgmRm3100Handler(mgm1Rm3100); + mgmRm3100Handler(mgm3Rm3100); + mgmLis3Handler(mgm0Lis3); + mgmLis3Handler(mgm2Lis3); + // To prevent task being not reactivated by tardy tasks + TaskFactory::delayTask(20); + } + return returnvalue::OK; +} + +ReturnValue_t AcsBoardPolling::initialize() { return returnvalue::OK; } + +ReturnValue_t AcsBoardPolling::initializeInterface(CookieIF* cookie) { + SpiCookie* spiCookie = dynamic_cast(cookie); + if (spiCookie == nullptr) { + return returnvalue::FAILED; + } + switch (spiCookie->getChipSelectPin()) { + case (gpioIds::MGM_0_LIS3_CS): { + mgm0Lis3.cookie = spiCookie; + break; + } + case (gpioIds::MGM_1_RM3100_CS): { + mgm1Rm3100.cookie = spiCookie; + break; + } + case (gpioIds::MGM_2_LIS3_CS): { + mgm2Lis3.cookie = spiCookie; + break; + } + case (gpioIds::MGM_3_RM3100_CS): { + mgm3Rm3100.cookie = spiCookie; + break; + } + case (gpioIds::GYRO_0_ADIS_CS): { + gyro0Adis.cookie = spiCookie; + break; + } + case (gpioIds::GYRO_1_L3G_CS): { + gyro1L3g.cookie = spiCookie; + break; + } + case (gpioIds::GYRO_2_ADIS_CS): { + gyro2Adis.cookie = spiCookie; + break; + } + case (gpioIds::GYRO_3_L3G_CS): { + gyro3L3g.cookie = spiCookie; + break; + } + default: { + sif::error << "AcsBoardPollingTask: invalid spi cookie" << std::endl; + } + } + return spiComIF.initializeInterface(cookie); +} + +ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* sendData, + size_t sendLen) { + SpiCookie* spiCookie = dynamic_cast(cookie); + if (spiCookie == nullptr) { + return returnvalue::FAILED; + } + auto handleAdisRequest = [&](GyroAdis& adis) { + if (sendLen != sizeof(acs::Adis1650XRequest)) { + sif::error << "AcsBoardPolling: invalid adis request send length"; + adis.replyResult = returnvalue::FAILED; + return returnvalue::FAILED; + } + auto* req = reinterpret_cast(sendData); + MutexGuard mg(ipcLock); + if (req->mode != adis.mode) { + if (req->mode == acs::SimpleSensorMode::NORMAL) { + adis.type = req->type; + adis.countdown.setTimeout(adis1650x::START_UP_TIME); + adis.countdown.resetTimer(); + if (adis.type == adis1650x::Type::ADIS16507) { + adis.ownReply.data.accelScaling = adis1650x::ACCELEROMETER_RANGE_16507; + } else if (adis.type == adis1650x::Type::ADIS16505) { + adis.ownReply.data.accelScaling = adis1650x::ACCELEROMETER_RANGE_16505; + } else { + sif::warning << "AcsBoardPolling: Unknown ADIS type" << std::endl; + } + adis.performStartup = true; + } else if (req->mode == acs::SimpleSensorMode::OFF) { + adis.performStartup = false; + adis.ownReply.cfgWasSet = false; + adis.ownReply.dataWasSet = false; + } + adis.mode = req->mode; + } + return returnvalue::OK; + }; + auto handleL3gRequest = [&](GyroL3g& gyro) { + if (sendLen != sizeof(acs::GyroL3gRequest)) { + sif::error << "AcsBoardPolling: invalid l3g request send length"; + gyro.replyResult = returnvalue::FAILED; + return returnvalue::FAILED; + } + auto* req = reinterpret_cast(sendData); + MutexGuard mg(ipcLock); + if (req->mode != gyro.mode) { + if (req->mode == acs::SimpleSensorMode::NORMAL) { + std::memcpy(gyro.sensorCfg, req->ctrlRegs, 5); + gyro.performStartup = true; + } else { + gyro.ownReply.cfgWasSet = false; + } + gyro.mode = req->mode; + } + return returnvalue::OK; + }; + auto handleLis3Request = [&](MgmLis3& mgm) { + if (sendLen != sizeof(acs::MgmLis3Request)) { + sif::error << "AcsBoardPolling: invalid lis3 request send length"; + mgm.replyResult = returnvalue::FAILED; + return returnvalue::FAILED; + } + auto* req = reinterpret_cast(sendData); + MutexGuard mg(ipcLock); + if (req->mode != mgm.mode) { + if (req->mode == acs::SimpleSensorMode::NORMAL) { + mgm.performStartup = true; + } else { + mgm.ownReply.dataWasSet = false; + mgm.ownReply.temperatureWasSet = false; + } + mgm.mode = req->mode; + } + return returnvalue::OK; + }; + auto handleRm3100Request = [&](MgmRm3100& mgm) { + if (sendLen != sizeof(acs::MgmRm3100Request)) { + sif::error << "AcsBoardPolling: invalid rm3100 request send length"; + mgm.replyResult = returnvalue::FAILED; + return returnvalue::FAILED; + } + auto* req = reinterpret_cast(sendData); + MutexGuard mg(ipcLock); + if (req->mode != mgm.mode) { + if (req->mode == acs::SimpleSensorMode::NORMAL) { + mgm.performStartup = true; + } else { + mgm.ownReply.dataWasRead = false; + } + mgm.mode = req->mode; + } + return returnvalue::OK; + }; + switch (spiCookie->getChipSelectPin()) { + case (gpioIds::MGM_0_LIS3_CS): { + handleLis3Request(mgm0Lis3); + break; + } + case (gpioIds::MGM_1_RM3100_CS): { + handleRm3100Request(mgm1Rm3100); + break; + } + case (gpioIds::MGM_2_LIS3_CS): { + handleLis3Request(mgm2Lis3); + break; + } + case (gpioIds::MGM_3_RM3100_CS): { + handleRm3100Request(mgm3Rm3100); + break; + } + case (gpioIds::GYRO_0_ADIS_CS): { + handleAdisRequest(gyro0Adis); + break; + } + case (gpioIds::GYRO_2_ADIS_CS): { + handleAdisRequest(gyro2Adis); + break; + } + case (gpioIds::GYRO_1_L3G_CS): { + handleL3gRequest(gyro1L3g); + break; + } + case (gpioIds::GYRO_3_L3G_CS): { + handleL3gRequest(gyro3L3g); + break; + } + } + MutexGuard mg(ipcLock); + if (state == InternalState::IDLE) { + state = InternalState::BUSY; + semaphore->release(); + } + return returnvalue::OK; +} + +ReturnValue_t AcsBoardPolling::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; } + +ReturnValue_t AcsBoardPolling::requestReceiveMessage(CookieIF* cookie, size_t requestLen) { + return returnvalue::OK; +} + +ReturnValue_t AcsBoardPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, + size_t* size) { + SpiCookie* spiCookie = dynamic_cast(cookie); + if (spiCookie == nullptr) { + return returnvalue::FAILED; + } + MutexGuard mg(ipcLock); + auto handleAdisReply = [&](GyroAdis& gyro) { + std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::Adis1650XReply)); + *buffer = reinterpret_cast(&gyro.readerReply); + *size = sizeof(acs::Adis1650XReply); + return gyro.replyResult; + }; + auto handleL3gReply = [&](GyroL3g& gyro) { + std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::GyroL3gReply)); + *buffer = reinterpret_cast(&gyro.readerReply); + *size = sizeof(acs::GyroL3gReply); + return gyro.replyResult; + }; + auto handleRm3100Reply = [&](MgmRm3100& mgm) { + std::memcpy(&mgm.readerReply, &mgm.ownReply, sizeof(acs::MgmRm3100Reply)); + *buffer = reinterpret_cast(&mgm.readerReply); + *size = sizeof(acs::MgmRm3100Reply); + return mgm.replyResult; + }; + auto handleLis3Reply = [&](MgmLis3& mgm) { + std::memcpy(&mgm.readerReply, &mgm.ownReply, sizeof(acs::MgmLis3Reply)); + *buffer = reinterpret_cast(&mgm.readerReply); + *size = sizeof(acs::MgmLis3Reply); + return mgm.replyResult; + }; + switch (spiCookie->getChipSelectPin()) { + case (gpioIds::MGM_0_LIS3_CS): { + return handleLis3Reply(mgm0Lis3); + } + case (gpioIds::MGM_1_RM3100_CS): { + return handleRm3100Reply(mgm1Rm3100); + } + case (gpioIds::MGM_2_LIS3_CS): { + return handleLis3Reply(mgm2Lis3); + } + case (gpioIds::MGM_3_RM3100_CS): { + return handleRm3100Reply(mgm3Rm3100); + } + case (gpioIds::GYRO_0_ADIS_CS): { + return handleAdisReply(gyro0Adis); + } + case (gpioIds::GYRO_2_ADIS_CS): { + return handleAdisReply(gyro2Adis); + } + case (gpioIds::GYRO_1_L3G_CS): { + return handleL3gReply(gyro1L3g); + } + case (gpioIds::GYRO_3_L3G_CS): { + return handleL3gReply(gyro3L3g); + } + } + return returnvalue::OK; +} + +void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) { + ReturnValue_t result; + acs::SimpleSensorMode mode; + bool gyroPerformStartup; + { + MutexGuard mg(ipcLock); + mode = l3g.mode; + gyroPerformStartup = l3g.performStartup; + } + if (mode == acs::SimpleSensorMode::NORMAL) { + if (gyroPerformStartup) { + cmdBuf[0] = l3gd20h::CTRL_REG_1 | l3gd20h::AUTO_INCREMENT_MASK; + std::memcpy(cmdBuf.data() + 1, l3g.sensorCfg, 5); + result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6); + if (result != returnvalue::OK) { + l3g.replyResult = returnvalue::OK; + } + // Ignore useless reply and red config + cmdBuf[0] = l3gd20h::CTRL_REG_1 | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK; + std::memset(cmdBuf.data() + 1, 0, 5); + result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6); + if (result != returnvalue::OK) { + l3g.replyResult = returnvalue::OK; + } + result = spiComIF.readReceivedMessage(l3g.cookie, &rawReply, &dummy); + if (result != returnvalue::OK) { + l3g.replyResult = returnvalue::OK; + } + MutexGuard mg(ipcLock); + // Cross check configuration as verification that communication is working + for (uint8_t idx = 0; idx < 5; idx++) { + if (rawReply[idx + 1] != l3g.sensorCfg[idx]) { + sif::warning << "AcsBoardPolling: l3g config check missmatch" << std::endl; + l3g.replyResult = returnvalue::FAILED; + return; + } + } + l3g.performStartup = false; + l3g.ownReply.cfgWasSet = true; + l3g.ownReply.sensitivity = l3gd20h::ctrlReg4ToSensitivity(l3g.sensorCfg[3]); + } + cmdBuf[0] = l3gd20h::READ_START | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK; + std::memset(cmdBuf.data() + 1, 0, l3gd20h::READ_LEN); + result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), l3gd20h::READ_LEN + 1); + if (result != returnvalue::OK) { + l3g.replyResult = returnvalue::FAILED; + return; + } + result = spiComIF.readReceivedMessage(l3g.cookie, &rawReply, &dummy); + if (result != returnvalue::OK) { + l3g.replyResult = returnvalue::FAILED; + return; + } + MutexGuard mg(ipcLock); + // The regular read function always returns the full sensor config as well. Use that + // to verify communications. + for (uint8_t idx = 0; idx < 5; idx++) { + if (rawReply[idx + 1] != l3g.sensorCfg[idx]) { + sif::warning << "AcsBoardPolling: l3g config check missmatch" << std::endl; + l3g.replyResult = returnvalue::FAILED; + return; + } + } + l3g.ownReply.statusReg = rawReply[l3gd20h::STATUS_IDX]; + l3g.ownReply.angVelocities[0] = (rawReply[l3gd20h::OUT_X_H] << 8) | rawReply[l3gd20h::OUT_X_L]; + l3g.ownReply.angVelocities[1] = (rawReply[l3gd20h::OUT_Y_H] << 8) | rawReply[l3gd20h::OUT_Y_L]; + l3g.ownReply.angVelocities[2] = (rawReply[l3gd20h::OUT_Z_H] << 8) | rawReply[l3gd20h::OUT_Z_L]; + l3g.ownReply.tempOffsetRaw = rawReply[l3gd20h::TEMPERATURE_IDX]; + } +} + +ReturnValue_t AcsBoardPolling::readAdisCfg(SpiCookie& cookie, size_t transferLen) { + ReturnValue_t result = returnvalue::OK; + int retval = 0; + // Prepare transfer + int fileDescriptor = 0; + std::string device = spiComIF.getSpiDev(); + UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage"); + if (fileHelper.getOpenResult() != returnvalue::OK) { + return SpiComIF::OPENING_FILE_FAILED; + } + spi::SpiModes spiMode = spi::SpiModes::MODE_0; + uint32_t spiSpeed = 0; + cookie.getSpiParameters(spiMode, spiSpeed, nullptr); + spiComIF.setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed); + cookie.assignWriteBuffer(cmdBuf.data()); + cookie.setTransferSize(2); + + gpioId_t gpioId = cookie.getChipSelectPin(); + MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; + uint32_t timeoutMs = 0; + MutexIF* mutex = spiComIF.getCsMutex(); + cookie.getMutexParams(timeoutType, timeoutMs); + if (mutex == nullptr) { + sif::warning << "GyroADIS16507Handler::spiSendCallback: " + "Mutex or GPIO interface invalid" + << std::endl; + return returnvalue::FAILED; + } + + size_t idx = 0; + spi_ioc_transfer* transferStruct = cookie.getTransferStructHandle(); + uint64_t origTx = transferStruct->tx_buf; + uint64_t origRx = transferStruct->rx_buf; + while (idx < transferLen) { + result = mutex->lockMutex(timeoutType, timeoutMs); + if (result != returnvalue::OK) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "AcsBoardPolling: Failed to lock mutex" << std::endl; +#endif + return result; + } + // Pull SPI CS low. For now, no support for active high given + if (gpioId != gpio::NO_GPIO) { + gpioIF.pullLow(gpioId); + } + + // Execute transfer + // Initiate a full duplex SPI transfer. + retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie.getTransferStructHandle()); + if (retval < 0) { + utility::handleIoctlError("SpiComIF::sendMessage: ioctl error."); + result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED; + } +#if FSFW_HAL_SPI_WIRETAPPING == 1 + comIf->performSpiWiretapping(cookie); +#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */ + + if (gpioId != gpio::NO_GPIO) { + gpioIF.pullHigh(gpioId); + } + mutex->unlockMutex(); + + idx += 2; + transferStruct->tx_buf += 2; + transferStruct->rx_buf += 2; + if (idx < transferLen) { + usleep(adis1650x::STALL_TIME_MICROSECONDS); + } + } + transferStruct->tx_buf = origTx; + transferStruct->rx_buf = origRx; + cookie.setTransferSize(transferLen); + return returnvalue::OK; +} + +void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) { + ReturnValue_t result; + acs::SimpleSensorMode mode; + bool cdHasTimedOut = false; + bool mustPerformStartup = false; + { + MutexGuard mg(ipcLock); + mode = gyro.mode; + cdHasTimedOut = gyro.countdown.hasTimedOut(); + mustPerformStartup = gyro.performStartup; + } + if (mode == acs::SimpleSensorMode::NORMAL and cdHasTimedOut) { + if (mustPerformStartup) { + uint8_t regList[6]; + // Read configuration + regList[0] = adis1650x::DIAG_STAT_REG; + regList[1] = adis1650x::FILTER_CTRL_REG; + regList[2] = adis1650x::RANG_MDL_REG; + regList[3] = adis1650x::MSC_CTRL_REG; + regList[4] = adis1650x::DEC_RATE_REG; + regList[5] = adis1650x::PROD_ID_REG; + size_t transferLen = + adis1650x::prepareReadCommand(regList, sizeof(regList), cmdBuf.data(), cmdBuf.size()); + result = readAdisCfg(*gyro.cookie, transferLen); + if (result != returnvalue::OK) { + gyro.replyResult = result; + return; + } + result = spiComIF.readReceivedMessage(gyro.cookie, &rawReply, &dummy); + if (result != returnvalue::OK or rawReply == nullptr) { + gyro.replyResult = result; + return; + } + uint16_t prodId = (rawReply[12] << 8) | rawReply[13]; + if (((gyro.type == adis1650x::Type::ADIS16505) and (prodId != adis1650x::PROD_ID_16505)) or + ((gyro.type == adis1650x::Type::ADIS16507) and (prodId != adis1650x::PROD_ID_16507))) { + sif::warning << "AcsPollingTask: Invalid ADIS product ID " << prodId << std::endl; + gyro.replyResult = returnvalue::FAILED; + return; + } + MutexGuard mg(ipcLock); + gyro.ownReply.cfgWasSet = true; + gyro.ownReply.cfg.diagStat = (rawReply[2] << 8) | rawReply[3]; + gyro.ownReply.cfg.filterSetting = (rawReply[4] << 8) | rawReply[5]; + gyro.ownReply.cfg.rangMdl = (rawReply[6] << 8) | rawReply[7]; + gyro.ownReply.cfg.mscCtrlReg = (rawReply[8] << 8) | rawReply[9]; + gyro.ownReply.cfg.decRateReg = (rawReply[10] << 8) | rawReply[11]; + gyro.ownReply.cfg.prodId = prodId; + gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl); + gyro.performStartup = false; + } + // Read regular registers + std::memcpy(cmdBuf.data(), adis1650x::BURST_READ_ENABLE.data(), + adis1650x::BURST_READ_ENABLE.size()); + std::memset(cmdBuf.data() + 2, 0, 10 * 2); + result = spiComIF.sendMessage(gyro.cookie, cmdBuf.data(), adis1650x::SENSOR_READOUT_SIZE); + if (result != returnvalue::OK) { + gyro.replyResult = returnvalue::FAILED; + return; + } + result = spiComIF.readReceivedMessage(gyro.cookie, &rawReply, &dummy); + if (result != returnvalue::OK or rawReply == nullptr) { + gyro.replyResult = returnvalue::FAILED; + return; + } + uint16_t checksum = (rawReply[20] << 8) | rawReply[21]; + + // Now verify the read checksum with the expected checksum according to datasheet p. 20 + uint16_t calcChecksum = 0; + for (size_t idx = 2; idx < 20; idx++) { + calcChecksum += rawReply[idx]; + } + if (checksum != calcChecksum) { + sif::warning << "AcsPollingTask: Invalid ADIS reply checksum" << std::endl; + gyro.replyResult = returnvalue::FAILED; + return; + } + + auto burstMode = adis1650x::burstModeFromMscCtrl(gyro.ownReply.cfg.mscCtrlReg); + if (burstMode != adis1650x::BurstModes::BURST_16_BURST_SEL_0) { + sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Analysis for select burst mode" + " not implemented!" + << std::endl; + gyro.replyResult = returnvalue::FAILED; + return; + } + + MutexGuard mg(ipcLock); + gyro.ownReply.dataWasSet = true; + gyro.ownReply.cfg.diagStat = rawReply[2] << 8 | rawReply[3]; + gyro.ownReply.data.angVelocities[0] = (rawReply[4] << 8) | rawReply[5]; + gyro.ownReply.data.angVelocities[1] = (rawReply[6] << 8) | rawReply[7]; + gyro.ownReply.data.angVelocities[2] = (rawReply[8] << 8) | rawReply[9]; + + gyro.ownReply.data.accelerations[0] = (rawReply[10] << 8) | rawReply[11]; + gyro.ownReply.data.accelerations[1] = (rawReply[12] << 8) | rawReply[13]; + gyro.ownReply.data.accelerations[2] = (rawReply[14] << 8) | rawReply[15]; + + gyro.ownReply.data.temperatureRaw = (rawReply[16] << 8) | rawReply[17]; + } +} + +void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) { + ReturnValue_t result; + acs::SimpleSensorMode mode; + bool mustPerformStartup = false; + { + MutexGuard mg(ipcLock); + mode = mgm.mode; + mustPerformStartup = mgm.performStartup; + } + if (mode == acs::SimpleSensorMode::NORMAL) { + if (mustPerformStartup) { + // To check valid communication, read back identification + // register which should always be the same value. + cmdBuf[0] = mgmLis3::readCommand(mgmLis3::IDENTIFY_DEVICE_REG_ADDR); + cmdBuf[1] = 0x00; + result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2); + if (result != OK) { + mgm.replyResult = result; + return; + } + result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy); + if (result != OK) { + mgm.replyResult = result; + return; + } + if (rawReply[1] != mgmLis3::DEVICE_ID) { + sif::error << "AcsPollingTask: invalid MGM lis3 device ID" << std::endl; + mgm.replyResult = result; + return; + } + mgm.cfg[0] = mgmLis3::CTRL_REG1_DEFAULT; + mgm.cfg[1] = mgmLis3::CTRL_REG2_DEFAULT; + mgm.cfg[2] = mgmLis3::CTRL_REG3_DEFAULT; + mgm.cfg[3] = mgmLis3::CTRL_REG4_DEFAULT; + mgm.cfg[4] = mgmLis3::CTRL_REG5_DEFAULT; + cmdBuf[0] = mgmLis3::writeCommand(mgmLis3::CTRL_REG1, true); + std::memcpy(cmdBuf.data() + 1, mgm.cfg, 5); + result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 6); + if (result != OK) { + mgm.replyResult = result; + return; + } + // Done here. We can always read back config and data during periodic handling + mgm.performStartup = false; + } + cmdBuf[0] = mgmLis3::readCommand(mgmLis3::CTRL_REG1, true); + std::memset(cmdBuf.data() + 1, 0, mgmLis3::NR_OF_DATA_AND_CFG_REGISTERS); + result = + spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), mgmLis3::NR_OF_DATA_AND_CFG_REGISTERS + 1); + if (result != returnvalue::OK) { + mgm.replyResult = result; + return; + } + result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy); + if (result != returnvalue::OK) { + mgm.replyResult = result; + return; + } + // Verify communication by re-checking config + if (rawReply[1] != mgm.cfg[0] or rawReply[2] != mgm.cfg[1] or rawReply[3] != mgm.cfg[2] or + rawReply[4] != mgm.cfg[3] or rawReply[5] != mgm.cfg[4]) { + mgm.replyResult = result; + return; + } + { + MutexGuard mg(ipcLock); + mgm.ownReply.dataWasSet = true; + mgm.ownReply.sensitivity = mgmLis3::getSensitivityFactor(mgmLis3::getSensitivity(mgm.cfg[1])); + mgm.ownReply.mgmValuesRaw[0] = + (rawReply[mgmLis3::X_HIGHBYTE_IDX] << 8) | rawReply[mgmLis3::X_LOWBYTE_IDX]; + mgm.ownReply.mgmValuesRaw[1] = + (rawReply[mgmLis3::Y_HIGHBYTE_IDX] << 8) | rawReply[mgmLis3::Y_LOWBYTE_IDX]; + mgm.ownReply.mgmValuesRaw[2] = + (rawReply[mgmLis3::Z_HIGHBYTE_IDX] << 8) | rawReply[mgmLis3::Z_LOWBYTE_IDX]; + } + // Read tempetature + cmdBuf[0] = mgmLis3::readCommand(mgmLis3::TEMP_LOWBYTE, true); + result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 3); + if (result != returnvalue::OK) { + mgm.replyResult = result; + return; + } + result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy); + if (result != returnvalue::OK) { + mgm.replyResult = result; + return; + } + MutexGuard mg(ipcLock); + mgm.ownReply.temperatureWasSet = true; + mgm.ownReply.temperatureRaw = (rawReply[2] << 8) | rawReply[1]; + } +} + +void AcsBoardPolling::mgmRm3100Handler(MgmRm3100& mgm) { + ReturnValue_t result; + acs::SimpleSensorMode mode; + bool mustPerformStartup = false; + { + MutexGuard mg(ipcLock); + mode = mgm.mode; + mustPerformStartup = mgm.performStartup; + } + if (mode == acs::SimpleSensorMode::NORMAL) { + if (mustPerformStartup) { + // Configure CMM first + cmdBuf[0] = mgmRm3100::CMM_REGISTER; + cmdBuf[1] = mgmRm3100::CMM_VALUE; + result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2); + if (result != OK) { + mgm.replyResult = result; + return; + } + // Read back register + cmdBuf[0] = mgmRm3100::CMM_REGISTER | mgmRm3100::READ_MASK; + cmdBuf[1] = 0; + result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2); + if (result != OK) { + mgm.replyResult = result; + return; + } + result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy); + if (result != OK) { + mgm.replyResult = result; + return; + } + if (rawReply[1] != mgmRm3100::CMM_VALUE) { + sif::error << "AcsBoardPolling: MGM RM3100 read back CMM invalid" << std::endl; + mgm.replyResult = result; + return; + } + // Configure TMRC register + cmdBuf[0] = mgmRm3100::TMRC_REGISTER; + // hardcoded for now + cmdBuf[1] = mgm.tmrcValue; + result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2); + if (result != OK) { + mgm.replyResult = result; + return; + } + // Read back and verify value + cmdBuf[0] = mgmRm3100::TMRC_REGISTER | mgmRm3100::READ_MASK; + cmdBuf[1] = 0; + result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2); + if (result != OK) { + mgm.replyResult = result; + return; + } + result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy); + if (result != OK) { + mgm.replyResult = result; + return; + } + if (rawReply[1] != mgm.tmrcValue) { + sif::error << "AcsBoardPolling: MGM RM3100 read back TMRC invalid" << std::endl; + mgm.replyResult = result; + return; + } + mgm.performStartup = false; + } + // Regular read operation + cmdBuf[0] = mgmRm3100::MEASUREMENT_REG_START | mgmRm3100::READ_MASK; + std::memset(cmdBuf.data() + 1, 0, 9); + result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 10); + if (result != OK) { + mgm.replyResult = result; + return; + } + result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy); + if (result != OK) { + mgm.replyResult = result; + return; + } + MutexGuard mg(ipcLock); + for (uint8_t idx = 0; idx < 3; idx++) { + // Hardcoded, but note that the gain depends on the cycle count + // value which is configurable! + mgm.ownReply.scaleFactors[idx] = 1.0 / mgmRm3100::DEFAULT_GAIN; + } + mgm.ownReply.dataWasRead = true; + // Bitshift trickery to account for 24 bit signed value. + mgm.ownReply.mgmValuesRaw[0] = + ((rawReply[1] << 24) | (rawReply[2] << 16) | (rawReply[3] << 8)) >> 8; + mgm.ownReply.mgmValuesRaw[1] = + ((rawReply[4] << 24) | (rawReply[5] << 16) | (rawReply[6] << 8)) >> 8; + mgm.ownReply.mgmValuesRaw[2] = + ((rawReply[7] << 24) | (rawReply[8] << 16) | (rawReply[9] << 8)) >> 8; + } +} diff --git a/linux/devices/AcsBoardPolling.h b/linux/devices/AcsBoardPolling.h new file mode 100644 index 00000000..58c35786 --- /dev/null +++ b/linux/devices/AcsBoardPolling.h @@ -0,0 +1,89 @@ +#ifndef LINUX_DEVICES_ACSBOARDPOLLING_H_ +#define LINUX_DEVICES_ACSBOARDPOLLING_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +class AcsBoardPolling : public SystemObject, + public ExecutableObjectIF, + public DeviceCommunicationIF { + public: + AcsBoardPolling(object_id_t objectId, SpiComIF& lowLevelComIF, GpioIF& gpioIF); + + ReturnValue_t performOperation(uint8_t operationCode) override; + ReturnValue_t initialize() override; + + private: + enum class InternalState { IDLE, BUSY } state = InternalState::IDLE; + MutexIF* ipcLock; + SemaphoreIF* semaphore; + std::array cmdBuf; + std::array replyBuf; + + struct DevBase { + SpiCookie* cookie = nullptr; + bool performStartup = false; + acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF; + ReturnValue_t replyResult = returnvalue::OK; + }; + + struct GyroAdis : public DevBase { + adis1650x::Type type; + Countdown countdown; + acs::Adis1650XReply ownReply; + acs::Adis1650XReply readerReply; + }; + GyroAdis gyro0Adis{}; + GyroAdis gyro2Adis{}; + + struct GyroL3g : public DevBase { + uint8_t sensorCfg[5]; + acs::GyroL3gReply ownReply; + acs::GyroL3gReply readerReply; + }; + GyroL3g gyro1L3g{}; + GyroL3g gyro3L3g{}; + + struct MgmRm3100 : public DevBase { + uint8_t tmrcValue = mgmRm3100::TMRC_DEFAULT_37HZ_VALUE; + acs::MgmRm3100Reply ownReply; + acs::MgmRm3100Reply readerReply; + }; + MgmRm3100 mgm1Rm3100; + MgmRm3100 mgm3Rm3100; + + struct MgmLis3 : public DevBase { + uint8_t cfg[5]{}; + acs::MgmLis3Reply ownReply; + acs::MgmLis3Reply readerReply; + }; + MgmLis3 mgm0Lis3; + MgmLis3 mgm2Lis3; + + uint8_t* rawReply = nullptr; + size_t dummy = 0; + + SpiComIF& spiComIF; + GpioIF& gpioIF; + + 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 gyroL3gHandler(GyroL3g& l3g); + void gyroAdisHandler(GyroAdis& gyro); + void mgmLis3Handler(MgmLis3& mgm); + void mgmRm3100Handler(MgmRm3100& mgm); + // Special readout: 16us stall time between small 2 byte transfers. + ReturnValue_t readAdisCfg(SpiCookie& spiCookie, size_t transferLen); +}; + +#endif /* LINUX_DEVICES_ACSBOARDPOLLING_H_ */ diff --git a/linux/devices/CMakeLists.txt b/linux/devices/CMakeLists.txt index 22b39840..ad4e8efe 100644 --- a/linux/devices/CMakeLists.txt +++ b/linux/devices/CMakeLists.txt @@ -4,8 +4,13 @@ endif() target_sources( ${OBSW_NAME} - PRIVATE Max31865RtdPolling.cpp ScexUartReader.cpp ImtqPollingTask.cpp - ScexDleParser.cpp ScexHelper.cpp RwPollingTask.cpp) + PRIVATE Max31865RtdPolling.cpp + ScexUartReader.cpp + ImtqPollingTask.cpp + ScexDleParser.cpp + ScexHelper.cpp + RwPollingTask.cpp + AcsBoardPolling.cpp) add_subdirectory(ploc) diff --git a/linux/fsfwconfig/objects/systemObjectList.h b/linux/fsfwconfig/objects/systemObjectList.h index 6cb70a06..26124b76 100644 --- a/linux/fsfwconfig/objects/systemObjectList.h +++ b/linux/fsfwconfig/objects/systemObjectList.h @@ -47,7 +47,6 @@ enum sourceObjects : uint32_t { GPIO_IF = 0x49010005, /* Custom device handler */ - RW_POLLING_TASK = 0x49020005, /* 0x54 ('T') for test handlers */ TEST_TASK = 0x54694269, diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 98f65427..92df2453 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -3,13 +3,13 @@ #include #include #include -#include +#include #include #include #include -#include #include #include +#include #include #include #include @@ -854,7 +854,7 @@ void ThermalController::copyDevices() { { lp_var_t tempGyro0 = - lp_var_t(objects::GYRO_0_ADIS_HANDLER, ADIS1650X::TEMPERATURE); + lp_var_t(objects::GYRO_0_ADIS_HANDLER, adis1650x::TEMPERATURE); PoolReadGuard pg(&tempGyro0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() != returnvalue::OK) { sif::warning << "ThermalController: Failed to read gyro 0 temperature" << std::endl; @@ -867,7 +867,7 @@ void ThermalController::copyDevices() { } { - lp_var_t tempGyro1 = lp_var_t(objects::GYRO_1_L3G_HANDLER, L3GD20H::TEMPERATURE); + lp_var_t tempGyro1 = lp_var_t(objects::GYRO_1_L3G_HANDLER, l3gd20h::TEMPERATURE); PoolReadGuard pg(&tempGyro1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() != returnvalue::OK) { sif::warning << "ThermalController: Failed to read gyro 1 temperature" << std::endl; @@ -881,7 +881,7 @@ void ThermalController::copyDevices() { { lp_var_t tempGyro2 = - lp_var_t(objects::GYRO_2_ADIS_HANDLER, ADIS1650X::TEMPERATURE); + lp_var_t(objects::GYRO_2_ADIS_HANDLER, adis1650x::TEMPERATURE); PoolReadGuard pg(&tempGyro2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() != returnvalue::OK) { sif::warning << "ThermalController: Failed to read gyro 2 temperature" << std::endl; @@ -894,7 +894,7 @@ void ThermalController::copyDevices() { } { - lp_var_t tempGyro3 = lp_var_t(objects::GYRO_3_L3G_HANDLER, L3GD20H::TEMPERATURE); + lp_var_t tempGyro3 = lp_var_t(objects::GYRO_3_L3G_HANDLER, l3gd20h::TEMPERATURE); PoolReadGuard pg(&tempGyro3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() != returnvalue::OK) { sif::warning << "ThermalController: Failed to read gyro 3 temperature" << std::endl; @@ -908,7 +908,7 @@ void ThermalController::copyDevices() { { lp_var_t tempMgm0 = - lp_var_t(objects::MGM_0_LIS3_HANDLER, MGMLIS3MDL::TEMPERATURE_CELCIUS); + lp_var_t(objects::MGM_0_LIS3_HANDLER, mgmLis3::TEMPERATURE_CELCIUS); PoolReadGuard pg(&tempMgm0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() != returnvalue::OK) { sif::warning << "ThermalController: Failed to read MGM 0 temperature" << std::endl; @@ -922,7 +922,7 @@ void ThermalController::copyDevices() { { lp_var_t tempMgm2 = - lp_var_t(objects::MGM_2_LIS3_HANDLER, MGMLIS3MDL::TEMPERATURE_CELCIUS); + lp_var_t(objects::MGM_2_LIS3_HANDLER, mgmLis3::TEMPERATURE_CELCIUS); PoolReadGuard pg(&tempMgm2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() != returnvalue::OK) { sif::warning << "ThermalController: Failed to read MGM 2 temperature" << std::endl; diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index 49e3d5fa..6323b326 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 @@ -9,7 +10,6 @@ #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" #include "linux/devices/devicedefinitions/StarTrackerDefinitions.h" #include "mission/devices/devicedefinitions/GPSDefinitions.h" -#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h" #include "mission/devices/devicedefinitions/SusDefinitions.h" namespace ACS { @@ -27,14 +27,12 @@ class SensorValues { ReturnValue_t updateStr(); ReturnValue_t updateRw(); - MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set = - MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER); - RM3100::Rm3100PrimaryDataset mgm1Rm3100Set = - RM3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER); - MGMLIS3MDL::MgmPrimaryDataset mgm2Lis3Set = - MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER); - RM3100::Rm3100PrimaryDataset mgm3Rm3100Set = - RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER); + mgmLis3::MgmPrimaryDataset mgm0Lis3Set = mgmLis3::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER); + mgmRm3100::Rm3100PrimaryDataset mgm1Rm3100Set = + mgmRm3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER); + mgmLis3::MgmPrimaryDataset mgm2Lis3Set = mgmLis3::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER); + mgmRm3100::Rm3100PrimaryDataset mgm3Rm3100Set = + mgmRm3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER); imtq::RawMtmMeasurementNoTorque imtqMgmSet = imtq::RawMtmMeasurementNoTorque(objects::IMTQ_HANDLER); diff --git a/mission/core/pollingSeqTables.cpp b/mission/core/pollingSeqTables.cpp index 0b134d87..7ceab326 100644 --- a/mission/core/pollingSeqTables.cpp +++ b/mission/core/pollingSeqTables.cpp @@ -189,110 +189,7 @@ ReturnValue_t pst::pstTest(FixedTimeslotTaskIF *thisSequence) { ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg) { /* Length of a communication cycle */ uint32_t length = thisSequence->getPeriodMs(); - bool enableAside = true; - bool enableBside = true; - if (cfg.scheduleAcsBoard) { - if (enableAside) { - // A side - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - 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, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - 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); - thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_READ); - } - if (enableBside) { - // B side - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - 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, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - 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); - thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_READ); - } - } // SUS: 16 ms bool addSus0 = true; bool addSus1 = true; @@ -583,6 +480,113 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::GET_READ); } + bool enableAside = true; + bool enableBside = true; + if (cfg.scheduleAcsBoard) { + if (enableAside) { + // A side + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + 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_3_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + 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_3_PERIOD, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, + length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ); + } + if (enableBside) { + // B side + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + 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_3_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + 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_3_PERIOD, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, + length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ); + } + if (enableAside) { + thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, + length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, + length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::GET_READ); + } + if (enableBside) { + thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, + length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, + length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::GET_READ); + } + } + if (cfg.scheduleImtq) { // This is the MTM measurement cycle thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, diff --git a/mission/devices/CMakeLists.txt b/mission/devices/CMakeLists.txt index ce02c688..cb6c066a 100644 --- a/mission/devices/CMakeLists.txt +++ b/mission/devices/CMakeLists.txt @@ -14,7 +14,10 @@ target_sources( ImtqHandler.cpp HeaterHandler.cpp RadiationSensorHandler.cpp - GyroADIS1650XHandler.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 new file mode 100644 index 00000000..1a91d36f --- /dev/null +++ b/mission/devices/GyrAdis1650XHandler.cpp @@ -0,0 +1,225 @@ +#include "mission/devices/GyrAdis1650XHandler.h" + +#include "fsfw/action/HasActionsIF.h" +#include "fsfw/datapool/PoolReadGuard.h" +#include "mission/devices/devicedefinitions/acsPolling.h" + +GyrAdis1650XHandler::GyrAdis1650XHandler(object_id_t objectId, object_id_t deviceCommunication, + CookieIF *comCookie, adis1650x::Type type) + : DeviceHandlerBase(objectId, deviceCommunication, comCookie), + adisType(type), + primaryDataset(this), + configDataset(this), + breakCountdown() { + adisRequest.type = adisType; +} + +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) { + warningSwitch = true; + breakCountdown.setTimeout(adis1650x::START_UP_TIME); + commandExecuted = true; + } + if (breakCountdown.hasTimedOut()) { + updatePeriodicReply(true, adis1650x::REPLY); + if (goToNormalMode) { + setMode(MODE_NORMAL); + } else { + setMode(MODE_ON); + } + internalState = InternalState::NONE; + } + } +} + +void GyrAdis1650XHandler::doShutDown() { + if (internalState != InternalState::SHUTDOWN) { + commandExecuted = false; + primaryDataset.setValidity(false, true); + internalState = InternalState::SHUTDOWN; + } + if (internalState == InternalState::SHUTDOWN and commandExecuted) { + updatePeriodicReply(false, adis1650x::REPLY); + internalState = InternalState::NONE; + commandExecuted = false; + setMode(_MODE_POWER_DOWN); + } +} + +ReturnValue_t GyrAdis1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { + *id = adis1650x::REQUEST; + return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL); +} + +ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + switch (internalState) { + case (InternalState::STARTUP): { + *id = adis1650x::REQUEST; + return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL); + } + case (InternalState::SHUTDOWN): { + *id = adis1650x::REQUEST; + acs::Adis1650XRequest *request = reinterpret_cast(cmdBuf.data()); + request->mode = acs::SimpleSensorMode::OFF; + request->type = adisType; + return returnvalue::OK; + } + default: { + return NOTHING_TO_SEND; + } + } + return returnvalue::OK; +} + +ReturnValue_t GyrAdis1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, + size_t commandDataLen) { + return NOTHING_TO_SEND; +} + +void GyrAdis1650XHandler::fillCommandAndReplyMap() { + insertInCommandMap(adis1650x::REQUEST); + insertInReplyMap(adis1650x::REPLY, 5, nullptr, 0, true); +} + +ReturnValue_t GyrAdis1650XHandler::scanForReply(const uint8_t *start, size_t remainingSize, + DeviceCommandId_t *foundId, size_t *foundLen) { + if (breakCountdown.isBusy() or getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) { + return IGNORE_FULL_PACKET; + } + if (remainingSize != sizeof(acs::Adis1650XReply)) { + *foundLen = remainingSize; + return returnvalue::FAILED; + } + *foundId = adis1650x::REPLY; + *foundLen = remainingSize; + if (internalState == InternalState::SHUTDOWN) { + commandExecuted = true; + } + return returnvalue::OK; +} + +ReturnValue_t GyrAdis1650XHandler::interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) { + using namespace adis1650x; + const acs::Adis1650XReply *reply = reinterpret_cast(packet); + if (internalState == InternalState::STARTUP and reply->cfgWasSet) { + switch (adisType) { + case (adis1650x::Type::ADIS16507): { + if (reply->cfg.prodId != adis1650x::PROD_ID_16507) { + sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID " + << reply->cfg.prodId << "for ADIS type 16507" << std::endl; + return returnvalue::FAILED; + } + break; + } + case (adis1650x::Type::ADIS16505): { + if (reply->cfg.prodId != adis1650x::PROD_ID_16505) { + sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID " + << reply->cfg.prodId << "for ADIS type 16505" << std::endl; + return returnvalue::FAILED; + } + break; + } + } + PoolReadGuard rg(&configDataset); + configDataset.setValidity(true, true); + configDataset.mscCtrlReg = reply->cfg.mscCtrlReg; + configDataset.rangMdl = reply->cfg.rangMdl; + configDataset.diagStatReg = reply->cfg.diagStat; + configDataset.filterSetting = reply->cfg.filterSetting; + configDataset.decRateReg = reply->cfg.decRateReg; + commandExecuted = true; + } + if (reply->dataWasSet) { + { + PoolReadGuard rg(&configDataset); + configDataset.diagStatReg = reply->cfg.diagStat; + } + auto sensitivity = reply->data.sensitivity; + auto accelSensitivity = reply->data.accelScaling; + PoolReadGuard pg(&primaryDataset); + primaryDataset.setValidity(true, true); + primaryDataset.angVelocX = static_cast(reply->data.angVelocities[0]) * sensitivity; + primaryDataset.angVelocY = static_cast(reply->data.angVelocities[1]) * sensitivity; + primaryDataset.angVelocZ = static_cast(reply->data.angVelocities[2]) * sensitivity; + // TODO: Check whether we need to divide by INT16_MAX + primaryDataset.accelX = static_cast(reply->data.accelerations[0]) * accelSensitivity; + primaryDataset.accelY = static_cast(reply->data.accelerations[1]) * accelSensitivity; + primaryDataset.accelZ = static_cast(reply->data.accelerations[2]) * accelSensitivity; + primaryDataset.temperature.value = static_cast(reply->data.temperatureRaw) * 0.1; + } + return returnvalue::OK; +} + +LocalPoolDataSetBase *GyrAdis1650XHandler::getDataSetHandle(sid_t sid) { + if (sid.ownerSetId == adis1650x::ADIS_DATASET_ID) { + return &primaryDataset; + } else if (sid.ownerSetId == adis1650x::ADIS_CFG_DATASET_ID) { + return &configDataset; + } + return nullptr; +} + +uint32_t GyrAdis1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 6000; } + +void GyrAdis1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne, + uint8_t valueTwo) { + uint8_t secondReg = startReg + 1; + startReg |= adis1650x::WRITE_MASK; + secondReg |= adis1650x::WRITE_MASK; + cmdBuf[0] = startReg; + cmdBuf[1] = valueOne; + cmdBuf[2] = secondReg; + cmdBuf[3] = valueTwo; + this->rawPacketLen = 4; + this->rawPacket = cmdBuf.data(); +} + +ReturnValue_t GyrAdis1650XHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(adis1650x::ANG_VELOC_X, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ANG_VELOC_Y, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ANG_VELOC_Z, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ACCELERATION_X, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ACCELERATION_Y, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ACCELERATION_Z, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::TEMPERATURE, new PoolEntry({0.0})); + + localDataPoolMap.emplace(adis1650x::DIAG_STAT_REGISTER, new PoolEntry()); + localDataPoolMap.emplace(adis1650x::FILTER_SETTINGS, new PoolEntry()); + localDataPoolMap.emplace(adis1650x::RANG_MDL, &rangMdl); + localDataPoolMap.emplace(adis1650x::MSC_CTRL_REGISTER, new PoolEntry()); + localDataPoolMap.emplace(adis1650x::DEC_RATE_REGISTER, new PoolEntry()); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(primaryDataset.getSid(), false, 5.0)); + return returnvalue::OK; +} + +adis1650x::BurstModes GyrAdis1650XHandler::getBurstMode() { + using namespace adis1650x; + configDataset.mscCtrlReg.read(); + uint16_t currentCtrlReg = configDataset.mscCtrlReg.value; + configDataset.mscCtrlReg.commit(); + return adis1650x::burstModeFromMscCtrl(currentCtrlReg); +} + +void GyrAdis1650XHandler::setToGoToNormalModeImmediately() { goToNormalMode = true; } + +void GyrAdis1650XHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) { + periodicPrintout = enable; + debugDivider.setDivider(divider); +} + +ReturnValue_t GyrAdis1650XHandler::preparePeriodicRequest(acs::SimpleSensorMode mode) { + adisRequest.mode = mode; + rawPacket = reinterpret_cast(&adisRequest); + rawPacketLen = sizeof(acs::Adis1650XRequest); + return returnvalue::OK; +} diff --git a/mission/devices/GyroADIS1650XHandler.h b/mission/devices/GyrAdis1650XHandler.h similarity index 70% rename from mission/devices/GyroADIS1650XHandler.h rename to mission/devices/GyrAdis1650XHandler.h index d348baa2..020dcd6e 100644 --- a/mission/devices/GyroADIS1650XHandler.h +++ b/mission/devices/GyrAdis1650XHandler.h @@ -1,27 +1,24 @@ #ifndef MISSION_DEVICES_GYROADIS16507HANDLER_H_ #define MISSION_DEVICES_GYROADIS16507HANDLER_H_ +#include +#include + #include "FSFWConfig.h" #include "OBSWConfig.h" -#include "devicedefinitions/GyroADIS1650XDefinitions.h" #include "fsfw/devicehandlers/DeviceHandlerBase.h" #include "fsfw/globalfunctions/PeriodicOperationDivider.h" -#ifdef FSFW_OSAL_LINUX -class SpiComIF; -class SpiCookie; -#endif - /** * @brief Device handle for the ADIS16507 Gyroscope by Analog Devices * @details * Flight manual: * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/ADIS16507_Gyro */ -class GyroADIS1650XHandler : public DeviceHandlerBase { +class GyrAdis1650XHandler : public DeviceHandlerBase { public: - GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, - ADIS1650X::Type type); + GyrAdis1650XHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, + adis1650x::Type type); void enablePeriodicPrintouts(bool enable, uint8_t divider); void setToGoToNormalModeImmediately(); @@ -40,42 +37,31 @@ class GyroADIS1650XHandler : public DeviceHandlerBase { uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) override; + LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override; private: - std::array commandBuffer; - ADIS1650X::Type adisType; + std::array cmdBuf; + acs::Adis1650XRequest adisRequest; + adis1650x::Type adisType; AdisGyroPrimaryDataset primaryDataset; AdisGyroConfigDataset configDataset; - double sensitivity = ADIS1650X::SENSITIVITY_UNSET; + double sensitivity = adis1650x::SENSITIVITY_UNSET; bool goToNormalMode = false; bool warningSwitch = true; - enum class InternalState { STARTUP, CONFIG, IDLE }; - - enum class BurstModes { - BURST_16_BURST_SEL_0, - BURST_16_BURST_SEL_1, - BURST_32_BURST_SEL_0, - BURST_32_BURST_SEL_1 - }; + enum class InternalState { NONE, STARTUP, SHUTDOWN }; InternalState internalState = InternalState::STARTUP; bool commandExecuted = false; PoolEntry rangMdl = PoolEntry(); - void prepareReadCommand(uint8_t *regList, size_t len); - - BurstModes getBurstMode(); - -#ifdef FSFW_OSAL_LINUX - static ReturnValue_t spiSendCallback(SpiComIF *comIf, SpiCookie *cookie, const uint8_t *sendData, - size_t sendLen, void *args); -#endif + adis1650x::BurstModes getBurstMode(); Countdown breakCountdown; void prepareWriteCommand(uint8_t startReg, uint8_t valueOne, uint8_t valueTwo); + ReturnValue_t preparePeriodicRequest(acs::SimpleSensorMode mode); ReturnValue_t handleSensorData(const uint8_t *packet); diff --git a/mission/devices/GyrL3gCustomHandler.cpp b/mission/devices/GyrL3gCustomHandler.cpp new file mode 100644 index 00000000..4bd5069f --- /dev/null +++ b/mission/devices/GyrL3gCustomHandler.cpp @@ -0,0 +1,191 @@ + +#include + +#include + +#include "fsfw/datapool/PoolReadGuard.h" +#include "mission/devices/devicedefinitions/acsPolling.h" + +GyrL3gCustomHandler::GyrL3gCustomHandler(object_id_t objectId, object_id_t deviceCommunication, + CookieIF *comCookie, uint32_t transitionDelayMs) + : DeviceHandlerBase(objectId, deviceCommunication, comCookie), + transitionDelayMs(transitionDelayMs), + dataset(this) {} + +GyrL3gCustomHandler::~GyrL3gCustomHandler() = default; + +void GyrL3gCustomHandler::doStartUp() { + if (internalState != InternalState::STARTUP) { + internalState = InternalState::STARTUP; + updatePeriodicReply(true, l3gd20h::REPLY); + commandExecuted = false; + } + + if (internalState == InternalState::STARTUP) { + if (commandExecuted) { + if (goNormalModeImmediately) { + setMode(MODE_NORMAL); + } else { + setMode(_MODE_TO_ON); + } + internalState = InternalState::NONE; + commandExecuted = false; + } + } +} + +void GyrL3gCustomHandler::doShutDown() { + if (internalState != InternalState::SHUTDOWN) { + internalState = InternalState::SHUTDOWN; + dataset.setValidity(false, true); + commandExecuted = false; + } + if (internalState == InternalState::SHUTDOWN and commandExecuted) { + internalState = InternalState::NONE; + updatePeriodicReply(false, l3gd20h::REPLY); + commandExecuted = false; + setMode(_MODE_POWER_DOWN); + } +} + +ReturnValue_t GyrL3gCustomHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + switch (internalState) { + case (InternalState::NONE): + case (InternalState::NORMAL): { + return NOTHING_TO_SEND; + } + case (InternalState::STARTUP): { + *id = l3gd20h::REQUEST; + gyrRequest.ctrlRegs[0] = l3gd20h::CTRL_REG_1_VAL; + gyrRequest.ctrlRegs[1] = l3gd20h::CTRL_REG_2_VAL; + gyrRequest.ctrlRegs[2] = l3gd20h::CTRL_REG_3_VAL; + gyrRequest.ctrlRegs[3] = l3gd20h::CTRL_REG_4_VAL; + gyrRequest.ctrlRegs[4] = l3gd20h::CTRL_REG_5_VAL; + return doPeriodicRequest(acs::SimpleSensorMode::NORMAL); + } + case (InternalState::SHUTDOWN): { + *id = l3gd20h::REQUEST; + return doPeriodicRequest(acs::SimpleSensorMode::OFF); + } + default: +#if FSFW_CPP_OSTREAM_ENABLED == 1 + /* Might be a configuration error. */ + sif::warning << "GyroL3GD20Handler::buildTransitionDeviceCommand: " + "Unknown internal state!" + << std::endl; +#else + sif::printDebug( + "GyroL3GD20Handler::buildTransitionDeviceCommand: " + "Unknown internal state!\n"); +#endif + return returnvalue::OK; + } + return returnvalue::OK; +} + +ReturnValue_t GyrL3gCustomHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { + *id = l3gd20h::REQUEST; + return doPeriodicRequest(acs::SimpleSensorMode::NORMAL); +} + +ReturnValue_t GyrL3gCustomHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, + size_t commandDataLen) { + switch (deviceCommand) { + default: + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } + return returnvalue::OK; +} + +ReturnValue_t GyrL3gCustomHandler::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::GyroL3gReply)) { + *foundLen = len; + return returnvalue::FAILED; + } + *foundId = l3gd20h::REPLY; + *foundLen = len; + if (internalState == InternalState::SHUTDOWN) { + commandExecuted = true; + } + return returnvalue::OK; +} + +ReturnValue_t GyrL3gCustomHandler::interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) { + const acs::GyroL3gReply *reply = reinterpret_cast(packet); + if (reply->cfgWasSet) { + if (internalState == InternalState::STARTUP) { + commandExecuted = true; + } + PoolReadGuard pg(&dataset); + dataset.setValidity(true, true); + dataset.angVelocX = static_cast(reply->angVelocities[0]) * reply->sensitivity; + dataset.angVelocY = static_cast(reply->angVelocities[1]) * reply->sensitivity; + dataset.angVelocZ = static_cast(reply->angVelocities[2]) * reply->sensitivity; + if (std::abs(dataset.angVelocX.value) > absLimitX) { + dataset.angVelocX.setValid(false); + } + if (std::abs(dataset.angVelocY.value) > absLimitY) { + dataset.angVelocY.setValid(false); + } + if (std::abs(dataset.angVelocZ.value) > absLimitZ) { + dataset.angVelocZ.setValid(false); + } + dataset.temperature = 25.0 - reply->tempOffsetRaw; + } + return returnvalue::OK; +} + +uint32_t GyrL3gCustomHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { + return this->transitionDelayMs; +} + +void GyrL3gCustomHandler::setToGoToNormalMode(bool enable) { this->goNormalModeImmediately = true; } + +ReturnValue_t GyrL3gCustomHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(l3gd20h::ANG_VELOC_X, new PoolEntry({0.0})); + localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Y, new PoolEntry({0.0})); + localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Z, new PoolEntry({0.0})); + localDataPoolMap.emplace(l3gd20h::TEMPERATURE, new PoolEntry({0.0})); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(dataset.getSid(), false, 10.0)); + return returnvalue::OK; +} + +void GyrL3gCustomHandler::fillCommandAndReplyMap() { + insertInCommandMap(l3gd20h::REQUEST); + insertInReplyMap(l3gd20h::REPLY, 5, nullptr, 0, true); +} + +void GyrL3gCustomHandler::modeChanged() { internalState = InternalState::NONE; } + +void GyrL3gCustomHandler::setAbsoluteLimits(float limitX, float limitY, float limitZ) { + this->absLimitX = limitX; + this->absLimitY = limitY; + this->absLimitZ = limitZ; +} + +void GyrL3gCustomHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) { + periodicPrintout = enable; + debugDivider.setDivider(divider); +} + +LocalPoolDataSetBase *GyrL3gCustomHandler::getDataSetHandle(sid_t sid) { + if (sid == dataset.getSid()) { + return &dataset; + } + return nullptr; +} + +ReturnValue_t GyrL3gCustomHandler::doPeriodicRequest(acs::SimpleSensorMode mode) { + gyrRequest.mode = mode; + rawPacket = reinterpret_cast(&gyrRequest); + rawPacketLen = sizeof(acs::GyroL3gRequest); + return returnvalue::OK; +} diff --git a/mission/devices/GyrL3gCustomHandler.h b/mission/devices/GyrL3gCustomHandler.h new file mode 100644 index 00000000..5f840cfc --- /dev/null +++ b/mission/devices/GyrL3gCustomHandler.h @@ -0,0 +1,91 @@ +#ifndef MISSION_DEVICES_GYROL3GD20HANDLER_H_ +#define MISSION_DEVICES_GYROL3GD20HANDLER_H_ + +#include +#include +#include +#include + +/** + * @brief Device Handler for the L3GD20H gyroscope sensor + * (https://www.st.com/en/mems-and-sensors/l3gd20h.html) + * @details + * Advanced documentation: + * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/L3GD20H_Gyro + * + * Data is read big endian with the smallest possible range of 245 degrees per second. + */ +class GyrL3gCustomHandler : public DeviceHandlerBase { + public: + GyrL3gCustomHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, + uint32_t transitionDelayMs); + virtual ~GyrL3gCustomHandler(); + + void enablePeriodicPrintouts(bool enable, uint8_t divider); + + /** + * Set the absolute limit for the values on the axis in degrees per second. + * The dataset values will be marked as invalid if that limit is exceeded + * @param xLimit + * @param yLimit + * @param zLimit + */ + void setAbsoluteLimits(float limitX, float limitY, float limitZ); + + /** + * @brief Configure device handler to go to normal mode immediately + */ + 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; + virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; + + void fillCommandAndReplyMap() override; + void modeChanged() override; + virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; + LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override; + + private: + uint32_t transitionDelayMs = 0; + GyroPrimaryDataset dataset; + + float absLimitX = l3gd20h::RANGE_DPS_00; + float absLimitY = l3gd20h::RANGE_DPS_00; + float absLimitZ = l3gd20h::RANGE_DPS_00; + + enum class InternalState { NONE, STARTUP, SHUTDOWN, NORMAL }; + InternalState internalState = InternalState::NONE; + bool commandExecuted = false; + + uint8_t statusReg = 0; + bool goNormalModeImmediately = false; + + uint8_t ctrlReg1Value = l3gd20h::CTRL_REG_1_VAL; + uint8_t ctrlReg2Value = l3gd20h::CTRL_REG_2_VAL; + uint8_t ctrlReg3Value = l3gd20h::CTRL_REG_3_VAL; + uint8_t ctrlReg4Value = l3gd20h::CTRL_REG_4_VAL; + uint8_t ctrlReg5Value = l3gd20h::CTRL_REG_5_VAL; + + acs::GyroL3gRequest gyrRequest{}; + + // Set default value + float sensitivity = l3gd20h::SENSITIVITY_00; + + bool periodicPrintout = false; + PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3); + + ReturnValue_t doPeriodicRequest(acs::SimpleSensorMode mode); +}; + +#endif /* MISSION_DEVICES_GYROL3GD20HANDLER_H_ */ diff --git a/mission/devices/GyroADIS1650XHandler.cpp b/mission/devices/GyroADIS1650XHandler.cpp deleted file mode 100644 index daff1b06..00000000 --- a/mission/devices/GyroADIS1650XHandler.cpp +++ /dev/null @@ -1,524 +0,0 @@ -#include "GyroADIS1650XHandler.h" - -#include -#include - -#include "fsfw/FSFW.h" - -#ifdef FSFW_OSAL_LINUX -#include -#include - -#include "fsfw_hal/linux/UnixFileGuard.h" -#include "fsfw_hal/linux/spi/SpiComIF.h" -#include "fsfw_hal/linux/spi/SpiCookie.h" -#include "fsfw_hal/linux/utility.h" -#endif - -GyroADIS1650XHandler::GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication, - CookieIF *comCookie, ADIS1650X::Type type) - : DeviceHandlerBase(objectId, deviceCommunication, comCookie), - adisType(type), - primaryDataset(this), - configDataset(this), - breakCountdown() { -#ifdef FSFW_OSAL_LINUX - SpiCookie *cookie = dynamic_cast(comCookie); - if (cookie != nullptr) { - cookie->setCallbackMode(&spiSendCallback, this); - } -#endif -} - -void GyroADIS1650XHandler::doStartUp() { - // Initial 310 ms start up time after power-up - if (internalState == InternalState::STARTUP) { - if (not commandExecuted) { - warningSwitch = true; - breakCountdown.setTimeout(ADIS1650X::START_UP_TIME); - commandExecuted = true; - } - if (breakCountdown.hasTimedOut()) { - internalState = InternalState::CONFIG; - commandExecuted = false; - } - } - - // Read all configuration registers first - if (internalState == InternalState::CONFIG) { - if (commandExecuted) { - commandExecuted = false; - internalState = InternalState::IDLE; - } - } - - if (internalState == InternalState::IDLE) { - if (goToNormalMode) { - setMode(MODE_NORMAL); - } else { - setMode(MODE_ON); - } - } -} - -void GyroADIS1650XHandler::doShutDown() { - commandExecuted = false; - internalState = InternalState::STARTUP; - setMode(_MODE_POWER_DOWN); -} - -ReturnValue_t GyroADIS1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { - *id = ADIS1650X::READ_SENSOR_DATA; - return buildCommandFromCommand(*id, nullptr, 0); -} - -ReturnValue_t GyroADIS1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { - switch (internalState) { - case (InternalState::CONFIG): { - *id = ADIS1650X::READ_OUT_CONFIG; - buildCommandFromCommand(*id, nullptr, 0); - break; - } - case (InternalState::STARTUP): { - return NOTHING_TO_SEND; - break; - } - default: { - // Might be a configuration error - sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: " - "Unknown internal state!" - << std::endl; - return returnvalue::OK; - } - } - return returnvalue::OK; -} - -ReturnValue_t GyroADIS1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, - const uint8_t *commandData, - size_t commandDataLen) { - switch (deviceCommand) { - case (ADIS1650X::READ_OUT_CONFIG): { - this->rawPacketLen = ADIS1650X::CONFIG_READOUT_SIZE; - uint8_t regList[6] = {}; - regList[0] = ADIS1650X::DIAG_STAT_REG; - regList[1] = ADIS1650X::FILTER_CTRL_REG; - regList[2] = ADIS1650X::RANG_MDL_REG; - regList[3] = ADIS1650X::MSC_CTRL_REG; - regList[4] = ADIS1650X::DEC_RATE_REG; - regList[5] = ADIS1650X::PROD_ID_REG; - prepareReadCommand(regList, sizeof(regList)); - this->rawPacket = commandBuffer.data(); - break; - } - case (ADIS1650X::READ_SENSOR_DATA): { - if (breakCountdown.isBusy()) { - // A glob command is pending and sensor data can't be read currently - return NO_REPLY_EXPECTED; - } - std::memcpy(commandBuffer.data(), ADIS1650X::BURST_READ_ENABLE.data(), - ADIS1650X::BURST_READ_ENABLE.size()); - std::memset(commandBuffer.data() + 2, 0, 10 * 2); - this->rawPacketLen = ADIS1650X::SENSOR_READOUT_SIZE; - this->rawPacket = commandBuffer.data(); - break; - } - case (ADIS1650X::SELF_TEST_SENSORS): { - if (breakCountdown.isBusy()) { - // Another glob command is pending - return HasActionsIF::IS_BUSY; - } - prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SENSOR_SELF_TEST, 0x00); - breakCountdown.setTimeout(ADIS1650X::SELF_TEST_BREAK); - break; - } - case (ADIS1650X::SELF_TEST_MEMORY): { - if (breakCountdown.isBusy()) { - // Another glob command is pending - return HasActionsIF::IS_BUSY; - } - prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_TEST, 0x00); - breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_TEST_BREAK); - break; - } - case (ADIS1650X::UPDATE_NV_CONFIGURATION): { - if (breakCountdown.isBusy()) { - // Another glob command is pending - return HasActionsIF::IS_BUSY; - } - prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_UPDATE, 0x00); - breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_UPDATE_BREAK); - break; - } - case (ADIS1650X::RESET_SENSOR_CONFIGURATION): { - if (breakCountdown.isBusy()) { - // Another glob command is pending - return HasActionsIF::IS_BUSY; - } - prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FACTORY_CALIBRATION, 0x00); - breakCountdown.setTimeout(ADIS1650X::FACTORY_CALIBRATION_BREAK); - break; - } - case (ADIS1650X::SW_RESET): { - if (breakCountdown.isBusy()) { - // Another glob command is pending - return HasActionsIF::IS_BUSY; - } - prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SOFTWARE_RESET, 0x00); - breakCountdown.setTimeout(ADIS1650X::SW_RESET_BREAK); - break; - } - case (ADIS1650X::PRINT_CURRENT_CONFIGURATION): { -#if OBSW_VERBOSE_LEVEL >= 1 - PoolReadGuard pg(&configDataset); - sif::info << "ADIS16507 Sensor configuration: DIAG_STAT: 0x" << std::hex << std::setw(4) - << std::setfill('0') << "0x" << configDataset.diagStatReg.value << std::endl; - sif::info << "MSC_CTRL: " << std::hex << std::setw(4) << "0x" - << configDataset.mscCtrlReg.value << " | FILT_CTRL: 0x" - << configDataset.filterSetting.value << " | DEC_RATE: 0x" - << configDataset.decRateReg.value << std::setfill(' ') << std::endl; -#endif /* OBSW_VERBOSE_LEVEL >= 1 */ - } - } - return returnvalue::OK; -} - -void GyroADIS1650XHandler::fillCommandAndReplyMap() { - insertInCommandAndReplyMap(ADIS1650X::READ_SENSOR_DATA, 1, &primaryDataset); - insertInCommandAndReplyMap(ADIS1650X::READ_OUT_CONFIG, 1, &configDataset); - insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_SENSORS, 1); - insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_MEMORY, 1); - insertInCommandAndReplyMap(ADIS1650X::UPDATE_NV_CONFIGURATION, 1); - insertInCommandAndReplyMap(ADIS1650X::RESET_SENSOR_CONFIGURATION, 1); - insertInCommandAndReplyMap(ADIS1650X::SW_RESET, 1); - insertInCommandAndReplyMap(ADIS1650X::PRINT_CURRENT_CONFIGURATION, 1); -} - -ReturnValue_t GyroADIS1650XHandler::scanForReply(const uint8_t *start, size_t remainingSize, - DeviceCommandId_t *foundId, size_t *foundLen) { - // For SPI, the ID will always be the one of the last sent command - *foundId = this->getPendingCommand(); - *foundLen = this->rawPacketLen; - - return returnvalue::OK; -} - -ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id, - const uint8_t *packet) { - using namespace ADIS1650X; - switch (id) { - case (ADIS1650X::READ_OUT_CONFIG): { - uint16_t readProdId = packet[12] << 8 | packet[13]; - if (((adisType == ADIS1650X::Type::ADIS16507) and (readProdId != ADIS1650X::PROD_ID_16507)) or - ((adisType == ADIS1650X::Type::ADIS16505) and (readProdId != ADIS1650X::PROD_ID_16505))) { -#if OBSW_VERBOSE_LEVEL >= 1 - if (warningSwitch) { - sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID " - << readProdId << std::endl; - } - warningSwitch = false; -#endif - return returnvalue::FAILED; - } - PoolReadGuard rg(&configDataset); - configDataset.diagStatReg.value = packet[2] << 8 | packet[3]; - configDataset.filterSetting.value = packet[4] << 8 | packet[5]; - uint16_t rangMdlRaw = packet[6] << 8 | packet[7]; - ADIS1650X::RangMdlBitfield bitfield = - static_cast((rangMdlRaw >> 2) & 0b11); - switch (bitfield) { - case (ADIS1650X::RangMdlBitfield::RANGE_125_1BMLZ): { - sensitivity = SENSITIVITY_1BMLZ; - break; - } - case (ADIS1650X::RangMdlBitfield::RANGE_500_2BMLZ): { - sensitivity = SENSITIVITY_2BMLZ; - break; - } - case (ADIS1650X::RangMdlBitfield::RANGE_2000_3BMLZ): { - sensitivity = SENSITIVITY_3BMLZ; - break; - } - case (RangMdlBitfield::RESERVED): { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "ADIS1650X: Unexpected value for RANG_MDL register" << std::endl; -#endif - break; - } - } - configDataset.rangMdl.value = rangMdlRaw; - configDataset.mscCtrlReg.value = packet[8] << 8 | packet[9]; - configDataset.decRateReg.value = packet[10] << 8 | packet[11]; - configDataset.setValidity(true, true); - if (internalState == InternalState::CONFIG) { - commandExecuted = true; - } - break; - } - case (ADIS1650X::READ_SENSOR_DATA): { - return handleSensorData(packet); - } - } - return returnvalue::OK; -} - -ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) { - BurstModes burstMode = getBurstMode(); - switch (burstMode) { - case (BurstModes::BURST_16_BURST_SEL_1): - case (BurstModes::BURST_32_BURST_SEL_1): { - sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Analysis with BURST_SEL1" - " not implemented!" - << std::endl; - return returnvalue::OK; - } - case (BurstModes::BURST_16_BURST_SEL_0): { - uint16_t checksum = packet[20] << 8 | packet[21]; - // Now verify the read checksum with the expected checksum according to datasheet p. 20 - uint16_t calcChecksum = 0; - for (size_t idx = 2; idx < 20; idx++) { - calcChecksum += packet[idx]; - } - if (checksum != calcChecksum) { -#if OBSW_VERBOSE_LEVEL >= 1 - sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: " - "Invalid checksum detected!" - << std::endl; -#endif - return returnvalue::FAILED; - } - - ReturnValue_t result = configDataset.diagStatReg.read(); - if (result == returnvalue::OK) { - configDataset.diagStatReg.value = packet[2] << 8 | packet[3]; - configDataset.diagStatReg.setValid(true); - } - configDataset.diagStatReg.commit(); - - { - PoolReadGuard pg(&primaryDataset); - int16_t angVelocXRaw = packet[4] << 8 | packet[5]; - primaryDataset.angVelocX.value = static_cast(angVelocXRaw) * sensitivity; - int16_t angVelocYRaw = packet[6] << 8 | packet[7]; - primaryDataset.angVelocY.value = static_cast(angVelocYRaw) * sensitivity; - int16_t angVelocZRaw = packet[8] << 8 | packet[9]; - primaryDataset.angVelocZ.value = static_cast(angVelocZRaw) * sensitivity; - - float accelScaling = 0; - if (adisType == ADIS1650X::Type::ADIS16507) { - accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16507; - } else if (adisType == ADIS1650X::Type::ADIS16505) { - accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16505; - } else { - sif::warning << "GyroADIS1650XHandler::handleSensorData: " - "Unknown ADIS type" - << std::endl; - } - int16_t accelXRaw = packet[10] << 8 | packet[11]; - primaryDataset.accelX.value = static_cast(accelXRaw) / INT16_MAX * accelScaling; - int16_t accelYRaw = packet[12] << 8 | packet[13]; - primaryDataset.accelY.value = static_cast(accelYRaw) / INT16_MAX * accelScaling; - int16_t accelZRaw = packet[14] << 8 | packet[15]; - primaryDataset.accelZ.value = static_cast(accelZRaw) / INT16_MAX * accelScaling; - - int16_t temperatureRaw = packet[16] << 8 | packet[17]; - primaryDataset.temperature.value = static_cast(temperatureRaw) * 0.1; - // Ignore data counter for now - primaryDataset.setValidity(true, true); - } - - if (periodicPrintout) { - if (debugDivider.checkAndIncrement()) { - sif::info << "GyroADIS1650XHandler: Angular velocities in deg / s" << std::endl; - sif::info << "X: " << primaryDataset.angVelocX.value << std::endl; - sif::info << "Y: " << primaryDataset.angVelocY.value << std::endl; - sif::info << "Z: " << primaryDataset.angVelocZ.value << std::endl; - sif::info << "GyroADIS1650XHandler: Accelerations in m / s^2: " << std::endl; - sif::info << "X: " << primaryDataset.accelX.value << std::endl; - sif::info << "Y: " << primaryDataset.accelY.value << std::endl; - sif::info << "Z: " << primaryDataset.accelZ.value << std::endl; - } - } - - break; - } - case (BurstModes::BURST_32_BURST_SEL_0): { - break; - } - } - return returnvalue::OK; -} - -uint32_t GyroADIS1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 6000; } - -void GyroADIS1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne, - uint8_t valueTwo) { - uint8_t secondReg = startReg + 1; - startReg |= ADIS1650X::WRITE_MASK; - secondReg |= ADIS1650X::WRITE_MASK; - commandBuffer[0] = startReg; - commandBuffer[1] = valueOne; - commandBuffer[2] = secondReg; - commandBuffer[3] = valueTwo; - this->rawPacketLen = 4; - this->rawPacket = commandBuffer.data(); -} - -void GyroADIS1650XHandler::prepareReadCommand(uint8_t *regList, size_t len) { - for (size_t idx = 0; idx < len; idx++) { - commandBuffer[idx * 2] = regList[idx]; - commandBuffer[idx * 2 + 1] = 0x00; - } - commandBuffer[len * 2] = 0x00; - commandBuffer[len * 2 + 1] = 0x00; -} - -ReturnValue_t GyroADIS1650XHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, - LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_X, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Y, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Z, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::ACCELERATION_X, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Y, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Z, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::TEMPERATURE, new PoolEntry({0.0})); - - localDataPoolMap.emplace(ADIS1650X::DIAG_STAT_REGISTER, new PoolEntry()); - localDataPoolMap.emplace(ADIS1650X::FILTER_SETTINGS, new PoolEntry()); - localDataPoolMap.emplace(ADIS1650X::RANG_MDL, &rangMdl); - localDataPoolMap.emplace(ADIS1650X::MSC_CTRL_REGISTER, new PoolEntry()); - localDataPoolMap.emplace(ADIS1650X::DEC_RATE_REGISTER, new PoolEntry()); - poolManager.subscribeForRegularPeriodicPacket( - subdp::RegularHkPeriodicParams(primaryDataset.getSid(), false, 5.0)); - return returnvalue::OK; -} - -GyroADIS1650XHandler::BurstModes GyroADIS1650XHandler::getBurstMode() { - configDataset.mscCtrlReg.read(); - uint16_t currentCtrlReg = configDataset.mscCtrlReg.value; - configDataset.mscCtrlReg.commit(); - if ((currentCtrlReg & ADIS1650X::BURST_32_BIT) == ADIS1650X::BURST_32_BIT) { - if ((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) { - return BurstModes::BURST_32_BURST_SEL_1; - } else { - return BurstModes::BURST_32_BURST_SEL_0; - } - } else { - if ((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) { - return BurstModes::BURST_16_BURST_SEL_1; - } else { - return BurstModes::BURST_16_BURST_SEL_0; - } - } -} - -#ifdef FSFW_OSAL_LINUX - -ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie, - const uint8_t *sendData, size_t sendLen, - void *args) { - GyroADIS1650XHandler *handler = reinterpret_cast(args); - if (handler == nullptr) { - sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!" - << std::endl; - return returnvalue::FAILED; - } - DeviceCommandId_t currentCommand = handler->getPendingCommand(); - switch (currentCommand) { - case (ADIS1650X::READ_SENSOR_DATA): { - return comIf->performRegularSendOperation(cookie, sendData, sendLen); - } - case (ADIS1650X::READ_OUT_CONFIG): - default: { - ReturnValue_t result = returnvalue::OK; - int retval = 0; - // Prepare transfer - int fileDescriptor = 0; - std::string device = comIf->getSpiDev(); - UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage"); - if (fileHelper.getOpenResult() != returnvalue::OK) { - return SpiComIF::OPENING_FILE_FAILED; - } - spi::SpiModes spiMode = spi::SpiModes::MODE_0; - uint32_t spiSpeed = 0; - cookie->getSpiParameters(spiMode, spiSpeed, nullptr); - comIf->setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed); - cookie->assignWriteBuffer(sendData); - cookie->setTransferSize(2); - - gpioId_t gpioId = cookie->getChipSelectPin(); - GpioIF &gpioIF = comIf->getGpioInterface(); - MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; - uint32_t timeoutMs = 0; - MutexIF *mutex = comIf->getCsMutex(); - cookie->getMutexParams(timeoutType, timeoutMs); - if (mutex == nullptr) { -#if OBSW_VERBOSE_LEVEL >= 1 - sif::warning << "GyroADIS16507Handler::spiSendCallback: " - "Mutex or GPIO interface invalid" - << std::endl; - return returnvalue::FAILED; -#endif - } - - if (gpioId != gpio::NO_GPIO) { - result = mutex->lockMutex(timeoutType, timeoutMs); - if (result != returnvalue::OK) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl; -#endif - return result; - } - } - - size_t idx = 0; - spi_ioc_transfer *transferStruct = cookie->getTransferStructHandle(); - uint64_t origTx = transferStruct->tx_buf; - uint64_t origRx = transferStruct->rx_buf; - while (idx < sendLen) { - // Pull SPI CS low. For now, no support for active high given - if (gpioId != gpio::NO_GPIO) { - gpioIF.pullLow(gpioId); - } - - // Execute transfer - // Initiate a full duplex SPI transfer. - retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie->getTransferStructHandle()); - if (retval < 0) { - utility::handleIoctlError("SpiComIF::sendMessage: ioctl error."); - result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED; - } -#if FSFW_HAL_SPI_WIRETAPPING == 1 - comIf->performSpiWiretapping(cookie); -#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */ - - if (gpioId != gpio::NO_GPIO) { - gpioIF.pullHigh(gpioId); - } - - idx += 2; - if (idx < sendLen) { - usleep(ADIS1650X::STALL_TIME_MICROSECONDS); - } - - transferStruct->tx_buf += 2; - transferStruct->rx_buf += 2; - } - transferStruct->tx_buf = origTx; - transferStruct->rx_buf = origRx; - if (gpioId != gpio::NO_GPIO) { - mutex->unlockMutex(); - } - } - } - return returnvalue::OK; -} - -void GyroADIS1650XHandler::setToGoToNormalModeImmediately() { goToNormalMode = true; } - -void GyroADIS1650XHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) { - periodicPrintout = enable; - debugDivider.setDivider(divider); -} - -#endif /* OBSW_ADIS1650X_LINUX_COM_IF == 1 */ diff --git a/mission/devices/MgmLis3CustomHandler.cpp b/mission/devices/MgmLis3CustomHandler.cpp new file mode 100644 index 00000000..7968abd7 --- /dev/null +++ b/mission/devices/MgmLis3CustomHandler.cpp @@ -0,0 +1,150 @@ +#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) { + dataset.setValidity(false, true); + 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::MgmLis3Reply)) { + *foundLen = len; + return returnvalue::FAILED; + } + *foundId = REPLY; + *foundLen = len; + if (internalState == InternalState::SHUTDOWN) { + commandExecuted = true; + } + 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..621d81e4 --- /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..685de23d --- /dev/null +++ b/mission/devices/MgmRm3100CustomHandler.cpp @@ -0,0 +1,138 @@ +#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; + primaryDataset.setValidity(false, true); + 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; + if (internalState == InternalState::SHUTDOWN) { + commandExecuted = true; + } + 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; +} + +LocalPoolDataSetBase *MgmRm3100CustomHandler::getDataSetHandle(sid_t sid) { + if (sid == primaryDataset.getSid()) { + return &primaryDataset; + } + return nullptr; +} diff --git a/mission/devices/MgmRm3100CustomHandler.h b/mission/devices/MgmRm3100CustomHandler.h new file mode 100644 index 00000000..4c0c98b3 --- /dev/null +++ b/mission/devices/MgmRm3100CustomHandler.h @@ -0,0 +1,97 @@ +#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; + LocalPoolDataSetBase *getDataSetHandle(sid_t sid) 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); + bool periodicPrintout = false; + + 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 prepareRequest(acs::SimpleSensorMode mode); + PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3); +}; + +#endif /* MISSION_DEVICEHANDLING_MgmRm3100CustomHandler_H_ */ diff --git a/mission/devices/devicedefinitions/CMakeLists.txt b/mission/devices/devicedefinitions/CMakeLists.txt index b2394b2a..eab4d4cb 100644 --- a/mission/devices/devicedefinitions/CMakeLists.txt +++ b/mission/devices/devicedefinitions/CMakeLists.txt @@ -1,2 +1,2 @@ target_sources(${LIB_EIVE_MISSION} PRIVATE ScexDefinitions.cpp rwHelpers.cpp - imtqHelpers.cpp) + imtqHelpers.cpp gyroAdisHelpers.cpp) diff --git a/mission/devices/devicedefinitions/GyroL3GD20Definitions.h b/mission/devices/devicedefinitions/GyroL3GD20Definitions.h index 4c4543d0..43783c82 100644 --- a/mission/devices/devicedefinitions/GyroL3GD20Definitions.h +++ b/mission/devices/devicedefinitions/GyroL3GD20Definitions.h @@ -6,7 +6,7 @@ #include -namespace L3GD20H { +namespace l3gd20h { /* Actual size is 15 but we round up a bit */ static constexpr size_t MAX_BUFFER_SIZE = 16; @@ -104,27 +104,27 @@ static constexpr uint32_t GYRO_DATASET_ID = READ_REGS; enum GyroPoolIds : lp_id_t { ANG_VELOC_X, ANG_VELOC_Y, ANG_VELOC_Z, TEMPERATURE }; -} // namespace L3GD20H +} // namespace l3gd20h class GyroPrimaryDataset : public StaticLocalDataSet<5> { public: /** Constructor for data users like controllers */ GyroPrimaryDataset(object_id_t mgmId) - : StaticLocalDataSet(sid_t(mgmId, L3GD20H::GYRO_DATASET_ID)) { + : StaticLocalDataSet(sid_t(mgmId, l3gd20h::GYRO_DATASET_ID)) { setAllVariablesReadOnly(); } /* Angular velocities in degrees per second (DPS) */ - lp_var_t angVelocX = lp_var_t(sid.objectId, L3GD20H::ANG_VELOC_X, this); - lp_var_t angVelocY = lp_var_t(sid.objectId, L3GD20H::ANG_VELOC_Y, this); - lp_var_t angVelocZ = lp_var_t(sid.objectId, L3GD20H::ANG_VELOC_Z, this); - lp_var_t temperature = lp_var_t(sid.objectId, L3GD20H::TEMPERATURE, this); + lp_var_t angVelocX = lp_var_t(sid.objectId, l3gd20h::ANG_VELOC_X, this); + lp_var_t angVelocY = lp_var_t(sid.objectId, l3gd20h::ANG_VELOC_Y, this); + lp_var_t angVelocZ = lp_var_t(sid.objectId, l3gd20h::ANG_VELOC_Z, this); + lp_var_t temperature = lp_var_t(sid.objectId, l3gd20h::TEMPERATURE, this); private: friend class GyroHandlerL3GD20H; /** Constructor for the data creator */ GyroPrimaryDataset(HasLocalDataPoolIF* hkOwner) - : StaticLocalDataSet(hkOwner, L3GD20H::GYRO_DATASET_ID) {} + : StaticLocalDataSet(hkOwner, l3gd20h::GYRO_DATASET_ID) {} }; #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ */ diff --git a/mission/devices/devicedefinitions/acsPolling.h b/mission/devices/devicedefinitions/acsPolling.h new file mode 100644 index 00000000..7398c199 --- /dev/null +++ b/mission/devices/devicedefinitions/acsPolling.h @@ -0,0 +1,81 @@ +#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_ +#define MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_ + +#include "fsfw/thermal/tcsDefinitions.h" +#include "gyroAdisHelpers.h" + +namespace acs { + +enum SimpleSensorMode { NORMAL = 0, OFF = 1 }; + +struct Adis1650XRequest { + SimpleSensorMode mode; + adis1650x::Type type; +}; + +struct Adis1650XConfig { + uint16_t diagStat; + uint16_t filterSetting; + uint16_t rangMdl; + uint16_t mscCtrlReg; + uint16_t decRateReg; + uint16_t prodId; +}; + +struct Adis1650XData { + double sensitivity = 0.0; + // Angular velocities in all axes (X, Y and Z) + int16_t angVelocities[3]{}; + double accelScaling = 0.0; + // Accelerations in all axes + int16_t accelerations[3]{}; + int16_t temperatureRaw = thermal::INVALID_TEMPERATURE; +}; + +struct Adis1650XReply { + bool cfgWasSet = false; + Adis1650XConfig cfg; + bool dataWasSet = false; + Adis1650XData data; +}; + +struct GyroL3gRequest { + SimpleSensorMode mode = SimpleSensorMode::OFF; + uint8_t ctrlRegs[5]{}; +}; + +struct GyroL3gReply { + bool cfgWasSet = false; + uint8_t statusReg; + // Angular velocities in all axes (X, Y and Z) + int16_t angVelocities[3]{}; + int8_t tempOffsetRaw = 0; + uint8_t ctrlRegs[5]{}; + float sensitivity = 0.0; +}; + +struct MgmRm3100Request { + SimpleSensorMode mode = SimpleSensorMode::OFF; +}; + +struct MgmRm3100Reply { + bool dataWasRead = false; + float scaleFactors[3]{}; + int32_t mgmValuesRaw[3]{}; +}; + +struct MgmLis3Request { + SimpleSensorMode mode = SimpleSensorMode::OFF; +}; + +struct MgmLis3Reply { + bool dataWasSet = false; + float sensitivity = 0.0; + int16_t mgmValuesRaw[3]{}; + bool temperatureWasSet = false; + int16_t temperatureRaw = thermal::INVALID_TEMPERATURE; +}; + +} // namespace acs + +#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_ */ diff --git a/mission/devices/devicedefinitions/gyroAdisHelpers.cpp b/mission/devices/devicedefinitions/gyroAdisHelpers.cpp new file mode 100644 index 00000000..0f41b058 --- /dev/null +++ b/mission/devices/devicedefinitions/gyroAdisHelpers.cpp @@ -0,0 +1,54 @@ +#include "gyroAdisHelpers.h" + +size_t adis1650x::prepareReadCommand(const uint8_t* regList, size_t len, uint8_t* outBuf, + size_t maxLen) { + if (len * 2 + 2 > maxLen) { + return 0; + } + for (size_t idx = 0; idx < len; idx++) { + outBuf[idx * 2] = regList[idx]; + outBuf[idx * 2 + 1] = 0x00; + } + outBuf[len * 2] = 0x00; + outBuf[len * 2 + 1] = 0x00; + return len * 2 + 2; +} + +adis1650x::BurstModes adis1650x::burstModeFromMscCtrl(uint16_t mscCtrl) { + if ((mscCtrl & adis1650x::BURST_32_BIT) == adis1650x::BURST_32_BIT) { + if ((mscCtrl & adis1650x::BURST_SEL_BIT) == adis1650x::BURST_SEL_BIT) { + return BurstModes::BURST_32_BURST_SEL_1; + } else { + return BurstModes::BURST_32_BURST_SEL_0; + } + } else { + if ((mscCtrl & adis1650x::BURST_SEL_BIT) == adis1650x::BURST_SEL_BIT) { + return BurstModes::BURST_16_BURST_SEL_1; + } else { + return BurstModes::BURST_16_BURST_SEL_0; + } + } +} + +double adis1650x::rangMdlToSensitivity(uint16_t rangMdl) { + adis1650x::RangMdlBitfield bitfield = + static_cast((rangMdl >> 2) & 0b11); + switch (bitfield) { + case (adis1650x::RangMdlBitfield::RANGE_125_1BMLZ): { + return SENSITIVITY_1BMLZ; + } + case (adis1650x::RangMdlBitfield::RANGE_500_2BMLZ): { + return SENSITIVITY_2BMLZ; + } + case (adis1650x::RangMdlBitfield::RANGE_2000_3BMLZ): { + return SENSITIVITY_3BMLZ; + } + case (RangMdlBitfield::RESERVED): + default: { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "ADIS1650X: Unexpected value for RANG_MDL register" << std::endl; +#endif + return 0.0; + } + } +} diff --git a/mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h b/mission/devices/devicedefinitions/gyroAdisHelpers.h similarity index 72% rename from mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h rename to mission/devices/devicedefinitions/gyroAdisHelpers.h index 3cfda2d4..09f0aa1e 100644 --- a/mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h +++ b/mission/devices/devicedefinitions/gyroAdisHelpers.h @@ -6,9 +6,20 @@ #include "fsfw/datapoollocal/StaticLocalDataSet.h" #include "fsfw/devicehandlers/DeviceHandlerIF.h" -namespace ADIS1650X { +namespace adis1650x { -enum class Type { ADIS16505, ADIS16507 }; +enum class BurstModes { + BURST_16_BURST_SEL_0, + BURST_16_BURST_SEL_1, + BURST_32_BURST_SEL_0, + BURST_32_BURST_SEL_1 +}; + +size_t prepareReadCommand(const uint8_t* regList, size_t len, uint8_t* outBuf, size_t maxLen); +BurstModes burstModeFromMscCtrl(uint16_t mscCtrl); +double rangMdlToSensitivity(uint16_t rangMdl); + +enum class Type : uint8_t { ADIS16505, ADIS16507 }; static constexpr size_t MAXIMUM_REPLY_SIZE = 64; static constexpr uint8_t WRITE_MASK = 0b1000'0000; @@ -67,6 +78,9 @@ static constexpr DeviceCommandId_t RESET_SENSOR_CONFIGURATION = 30; static constexpr DeviceCommandId_t SW_RESET = 31; static constexpr DeviceCommandId_t PRINT_CURRENT_CONFIGURATION = 32; +static constexpr DeviceCommandId_t REQUEST = 0x70; +static constexpr DeviceCommandId_t REPLY = 0x77; + static constexpr uint16_t BURST_32_BIT = 1 << 9; static constexpr uint16_t BURST_SEL_BIT = 1 << 8; static constexpr uint16_t LIN_ACCEL_COMPENSATION_BIT = 1 << 7; @@ -111,57 +125,57 @@ enum FilterSettings : uint8_t { SIXTYFOUR_TAPS = 6 }; -} // namespace ADIS1650X +} // namespace adis1650x class AdisGyroPrimaryDataset : public StaticLocalDataSet<8> { public: /** Constructor for data users like controllers */ AdisGyroPrimaryDataset(object_id_t adisId) - : StaticLocalDataSet(sid_t(adisId, ADIS1650X::ADIS_DATASET_ID)) { + : StaticLocalDataSet(sid_t(adisId, adis1650x::ADIS_DATASET_ID)) { setAllVariablesReadOnly(); } /* Angular velocities in degrees per second (DPS) */ - lp_var_t angVelocX = lp_var_t(sid.objectId, ADIS1650X::ANG_VELOC_X, this); - lp_var_t angVelocY = lp_var_t(sid.objectId, ADIS1650X::ANG_VELOC_Y, this); - lp_var_t angVelocZ = lp_var_t(sid.objectId, ADIS1650X::ANG_VELOC_Z, this); - lp_var_t accelX = lp_var_t(sid.objectId, ADIS1650X::ACCELERATION_X, this); - lp_var_t accelY = lp_var_t(sid.objectId, ADIS1650X::ACCELERATION_Y, this); - lp_var_t accelZ = lp_var_t(sid.objectId, ADIS1650X::ACCELERATION_Z, this); - lp_var_t temperature = lp_var_t(sid.objectId, ADIS1650X::TEMPERATURE, this); + lp_var_t angVelocX = lp_var_t(sid.objectId, adis1650x::ANG_VELOC_X, this); + lp_var_t angVelocY = lp_var_t(sid.objectId, adis1650x::ANG_VELOC_Y, this); + lp_var_t angVelocZ = lp_var_t(sid.objectId, adis1650x::ANG_VELOC_Z, this); + lp_var_t accelX = lp_var_t(sid.objectId, adis1650x::ACCELERATION_X, this); + lp_var_t accelY = lp_var_t(sid.objectId, adis1650x::ACCELERATION_Y, this); + lp_var_t accelZ = lp_var_t(sid.objectId, adis1650x::ACCELERATION_Z, this); + lp_var_t temperature = lp_var_t(sid.objectId, adis1650x::TEMPERATURE, this); private: - friend class GyroADIS1650XHandler; + friend class GyrAdis1650XHandler; friend class GyroAdisDummy; /** Constructor for the data creator */ AdisGyroPrimaryDataset(HasLocalDataPoolIF* hkOwner) - : StaticLocalDataSet(hkOwner, ADIS1650X::ADIS_DATASET_ID) {} + : StaticLocalDataSet(hkOwner, adis1650x::ADIS_DATASET_ID) {} }; class AdisGyroConfigDataset : public StaticLocalDataSet<5> { public: /** Constructor for data users like controllers */ AdisGyroConfigDataset(object_id_t adisId) - : StaticLocalDataSet(sid_t(adisId, ADIS1650X::ADIS_CFG_DATASET_ID)) { + : StaticLocalDataSet(sid_t(adisId, adis1650x::ADIS_CFG_DATASET_ID)) { setAllVariablesReadOnly(); } lp_var_t diagStatReg = - lp_var_t(sid.objectId, ADIS1650X::DIAG_STAT_REGISTER, this); + lp_var_t(sid.objectId, adis1650x::DIAG_STAT_REGISTER, this); lp_var_t filterSetting = - lp_var_t(sid.objectId, ADIS1650X::FILTER_SETTINGS, this); - lp_var_t rangMdl = lp_var_t(sid.objectId, ADIS1650X::RANG_MDL, this); + lp_var_t(sid.objectId, adis1650x::FILTER_SETTINGS, this); + lp_var_t rangMdl = lp_var_t(sid.objectId, adis1650x::RANG_MDL, this); lp_var_t mscCtrlReg = - lp_var_t(sid.objectId, ADIS1650X::MSC_CTRL_REGISTER, this); + lp_var_t(sid.objectId, adis1650x::MSC_CTRL_REGISTER, this); lp_var_t decRateReg = - lp_var_t(sid.objectId, ADIS1650X::DEC_RATE_REGISTER, this); + lp_var_t(sid.objectId, adis1650x::DEC_RATE_REGISTER, this); private: - friend class GyroADIS1650XHandler; + friend class GyrAdis1650XHandler; /** Constructor for the data creator */ AdisGyroConfigDataset(HasLocalDataPoolIF* hkOwner) - : StaticLocalDataSet(hkOwner, ADIS1650X::ADIS_CFG_DATASET_ID) {} + : StaticLocalDataSet(hkOwner, adis1650x::ADIS_CFG_DATASET_ID) {} }; #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_ */ diff --git a/mission/system/objects/AcsBoardAssembly.cpp b/mission/system/objects/AcsBoardAssembly.cpp index bcc2eb7a..01dd76b1 100644 --- a/mission/system/objects/AcsBoardAssembly.cpp +++ b/mission/system/objects/AcsBoardAssembly.cpp @@ -274,4 +274,9 @@ void AcsBoardAssembly::refreshHelperModes() { } } -ReturnValue_t AcsBoardAssembly::initialize() { return AssemblyBase::initialize(); } +ReturnValue_t AcsBoardAssembly::initialize() { + for (const auto& child : childrenMap) { + updateChildModeByObjId(child.first, MODE_OFF, 0); + } + return AssemblyBase::initialize(); +} diff --git a/mission/system/objects/SusAssembly.cpp b/mission/system/objects/SusAssembly.cpp index 3f75a369..62a47616 100644 --- a/mission/system/objects/SusAssembly.cpp +++ b/mission/system/objects/SusAssembly.cpp @@ -120,7 +120,12 @@ ReturnValue_t SusAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wan return returnvalue::OK; } -ReturnValue_t SusAssembly::initialize() { return AssemblyBase::initialize(); } +ReturnValue_t SusAssembly::initialize() { + for (const auto& child : childrenMap) { + updateChildModeByObjId(child.first, MODE_OFF, 0); + } + return AssemblyBase::initialize(); +} bool SusAssembly::isUseable(object_id_t object, Mode_t mode) { if (healthHelper.healthTable->isFaulty(object)) {