From 828b6adf7739754d182b0feab90683e482f2c111 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 26 Feb 2023 14:55:33 +0100 Subject: [PATCH 1/9] vieles --- CHANGELOG.md | 4 + bsp_q7s/core/ObjectFactory.cpp | 36 +- bsp_q7s/core/ObjectFactory.h | 3 +- bsp_q7s/core/scheduling.cpp | 12 + bsp_q7s/fmObjectFactory.cpp | 2 +- common/config/eive/objects.h | 4 +- dummies/GyroAdisDummy.cpp | 16 +- dummies/GyroAdisDummy.h | 3 +- dummies/GyroL3GD20Dummy.cpp | 10 +- fsfw | 2 +- linux/devices/AcsBoardPolling.cpp | 453 +++++++++++++++ linux/devices/AcsBoardPolling.h | 81 +++ linux/devices/CMakeLists.txt | 9 +- linux/fsfwconfig/objects/systemObjectList.h | 1 - mission/controller/ThermalController.cpp | 10 +- mission/controller/acs/SensorValues.h | 2 +- mission/core/pollingSeqTables.cpp | 210 +++---- mission/devices/CMakeLists.txt | 3 +- mission/devices/GyrAdis1650XHandler.cpp | 515 +++++++++++++++++ ...IS1650XHandler.h => GyrAdis1650XHandler.h} | 42 +- mission/devices/GyrL3gCustomHandler.cpp | 216 ++++++++ mission/devices/GyrL3gCustomHandler.h | 92 +++ mission/devices/GyroADIS1650XHandler.cpp | 524 ------------------ .../devices/devicedefinitions/CMakeLists.txt | 2 +- .../devicedefinitions/GyroL3GD20Definitions.h | 16 +- .../devices/devicedefinitions/acsPolling.h | 58 ++ .../devicedefinitions/gyroAdisHelpers.cpp | 54 ++ ...IS1650XDefinitions.h => gyroAdisHelpers.h} | 56 +- mission/system/objects/AcsBoardAssembly.cpp | 7 +- mission/system/objects/SusAssembly.cpp | 7 +- tmtc | 2 +- 31 files changed, 1720 insertions(+), 732 deletions(-) create mode 100644 linux/devices/AcsBoardPolling.cpp create mode 100644 linux/devices/AcsBoardPolling.h create mode 100644 mission/devices/GyrAdis1650XHandler.cpp rename mission/devices/{GyroADIS1650XHandler.h => GyrAdis1650XHandler.h} (70%) create mode 100644 mission/devices/GyrL3gCustomHandler.cpp create mode 100644 mission/devices/GyrL3gCustomHandler.h delete mode 100644 mission/devices/GyroADIS1650XHandler.cpp create mode 100644 mission/devices/devicedefinitions/acsPolling.h create mode 100644 mission/devices/devicedefinitions/gyroAdisHelpers.cpp rename mission/devices/devicedefinitions/{GyroADIS1650XDefinitions.h => gyroAdisHelpers.h} (72%) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8b1b2ed5..1fdc3e1f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,10 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Changed + +- Move ACS board polling to separate worker thread. + # [v1.32.0] eive-tmtc: v2.16.1 diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 59a17207..c28806bc 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -1,8 +1,10 @@ #include "ObjectFactory.h" #include +#include #include #include +#include #include #include "OBSWConfig.h" @@ -57,6 +59,7 @@ #if OBSW_TEST_LIBGPIOD == 1 #include "linux/boardtest/LibgpiodTest.h" #endif +#include #include #include #include @@ -83,7 +86,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" @@ -239,8 +241,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(); @@ -343,6 +345,7 @@ 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 = @@ -414,12 +417,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; @@ -431,11 +434,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; @@ -448,11 +452,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; @@ -461,11 +466,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/fsfw b/fsfw index bdfe31db..cf735143 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit bdfe31dba48039b60fe700e7d03bfb95e9549688 +Subproject commit cf735143fe4b79db2fc7faba2b1cd239474e2cfc diff --git a/linux/devices/AcsBoardPolling.cpp b/linux/devices/AcsBoardPolling.cpp new file mode 100644 index 00000000..9803dfe9 --- /dev/null +++ b/linux/devices/AcsBoardPolling.cpp @@ -0,0 +1,453 @@ +#include "AcsBoardPolling.h" + +#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); + // 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): { + mgm0L3Cookie = spiCookie; + break; + } + case (gpioIds::MGM_1_RM3100_CS): { + mgm1Rm3100Cookie = spiCookie; + break; + } + case (gpioIds::MGM_2_LIS3_CS): { + mgm2L3Cookie = spiCookie; + break; + } + case (gpioIds::MGM_3_RM3100_CS): { + mgm3Rm3100Cookie = 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) { + gyro.performStartup = true; + } else { + gyro.ownReply.cfgWasSet = false; + } + gyro.mode = req->mode; + } + return returnvalue::OK; + }; + switch (spiCookie->getChipSelectPin()) { + 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); + }; + auto handleL3gReply = [&](GyroL3g& gyro) { + std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::GyroL3gReply)); + *buffer = reinterpret_cast(&gyro.readerReply); + *size = sizeof(acs::GyroL3gReply); + }; + switch (spiCookie->getChipSelectPin()) { + case (gpioIds::GYRO_0_ADIS_CS): { + handleAdisReply(gyro0Adis); + return gyro0Adis.replyResult; + } + case (gpioIds::GYRO_2_ADIS_CS): { + handleAdisReply(gyro2Adis); + return gyro2Adis.replyResult; + } + case (gpioIds::GYRO_1_L3G_CS): { + handleL3gReply(gyro1L3g); + return gyro1L3g.replyResult; + } + case (gpioIds::GYRO_3_L3G_CS): { + handleL3gReply(gyro3L3g); + return gyro3L3g.replyResult; + } + } + 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.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(), 6); + if (result != returnvalue::OK) { + l3g.replyResult = returnvalue::FAILED; + return; + } + // 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]; + } +} diff --git a/linux/devices/AcsBoardPolling.h b/linux/devices/AcsBoardPolling.h new file mode 100644 index 00000000..bacc2a84 --- /dev/null +++ b/linux/devices/AcsBoardPolling.h @@ -0,0 +1,81 @@ +#ifndef LINUX_DEVICES_ACSBOARDPOLLING_H_ +#define LINUX_DEVICES_ACSBOARDPOLLING_H_ + +#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; + + bool mgm0L3IsOn = false; + SpiCookie* mgm0L3Cookie = nullptr; + bool mgm1Rm3100IsOn = false; + SpiCookie* mgm1Rm3100Cookie = nullptr; + bool mgm2L3IsOn = false; + SpiCookie* mgm2L3Cookie = nullptr; + bool mgm3Rm3100IsOn = false; + SpiCookie* mgm3Rm3100Cookie = nullptr; + + struct GyroAdis { + adis1650x::Type type; + bool isOn = false; + bool performStartup = false; + SpiCookie* cookie = nullptr; + Countdown countdown; + acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF; + ReturnValue_t replyResult; + acs::Adis1650XReply ownReply; + acs::Adis1650XReply readerReply; + }; + GyroAdis gyro0Adis{}; + GyroAdis gyro2Adis{}; + + struct GyroL3g { + bool performStartup = false; + SpiCookie* cookie = nullptr; + acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF; + uint8_t sensorCfg[5]; + ReturnValue_t replyResult; + acs::GyroL3gReply ownReply; + acs::GyroL3gReply readerReply; + }; + GyroL3g gyro1L3g{}; + GyroL3g gyro3L3g{}; + + 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); + // 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..3097cb5e 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -7,9 +7,9 @@ #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; diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index 49e3d5fa..87a21589 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 { diff --git a/mission/core/pollingSeqTables.cpp b/mission/core/pollingSeqTables.cpp index 0b134d87..cdf086fd 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_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); + } + 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); + } + 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_3_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..adcbaf73 100644 --- a/mission/devices/CMakeLists.txt +++ b/mission/devices/CMakeLists.txt @@ -14,7 +14,8 @@ target_sources( ImtqHandler.cpp HeaterHandler.cpp RadiationSensorHandler.cpp - GyroADIS1650XHandler.cpp + GyrAdis1650XHandler.cpp + GyrL3gCustomHandler.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..7a13ec1a --- /dev/null +++ b/mission/devices/GyrAdis1650XHandler.cpp @@ -0,0 +1,515 @@ +#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() { + // 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); + } + } + } + + // // Read all configuration registers first + // if (internalState == InternalState::CONFIG) { + // if (commandExecuted) { + // commandExecuted = false; + // internalState = InternalState::IDLE; + // } + // } + // + // if (internalState == InternalState::IDLE) { + // } +} + +void GyrAdis1650XHandler::doShutDown() { + if (internalState != InternalState::SHUTDOWN) { + commandExecuted = false; + primaryDataset.setValidity(false, true); + internalState = InternalState::SHUTDOWN; + } + if (commandExecuted) { + updatePeriodicReply(false, adis1650x::REPLY); + 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: { + // Might be a configuration error + sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: " + "Unknown internal state!" + << std::endl; + return returnvalue::OK; + } + } + return returnvalue::OK; +} + +ReturnValue_t GyrAdis1650XHandler::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; + // adis1650x::prepareReadCommand(regList, sizeof(regList), commandBuffer.data(), + // commandBuffer.size()); + // 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; + // } + // TODO: Convert to special request + + // 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; + return NOTHING_TO_SEND; +} + +void GyrAdis1650XHandler::fillCommandAndReplyMap() { + insertInCommandMap(adis1650x::REQUEST); + insertInReplyMap(adis1650x::REPLY, 5, nullptr, 0, true); + // 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 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; + + 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; +} + +// ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) { +// using namespace adis1650x; +// 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 (adis1650x::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 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); +} + +//#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 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..3d8e82cf 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 { STARTUP, SHUTDOWN, IDLE }; 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..0af32d0c --- /dev/null +++ b/mission/devices/GyrL3gCustomHandler.cpp @@ -0,0 +1,216 @@ + +#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::NONE) { + 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); + } + } + } +} + +void GyrL3gCustomHandler::doShutDown() { + if (internalState != InternalState::SHUTDOWN) { + internalState = InternalState::SHUTDOWN; + commandExecuted = false; + } + if (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; + 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 readSet(&dataset); + dataset.setValidity(true, true); + dataset.angVelocX = reply->angVelocities[0]; + dataset.angVelocY = reply->angVelocities[1]; + dataset.angVelocZ = reply->angVelocities[2]; + 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; + } + // PoolReadGuard readSet(&dataset); + // if (readSet.getReadResult() == returnvalue::OK) { + // if (std::abs(angVelocX) < this->absLimitX) { + // dataset.angVelocX = angVelocX; + // dataset.angVelocX.setValid(true); + // } else { + // dataset.angVelocX.setValid(false); + // } + // + // if (std::abs(angVelocY) < this->absLimitY) { + // dataset.angVelocY = angVelocY; + // dataset.angVelocY.setValid(true); + // } else { + // dataset.angVelocY.setValid(false); + // } + // + // if (std::abs(angVelocZ) < this->absLimitZ) { + // dataset.angVelocZ = angVelocZ; + // dataset.angVelocZ.setValid(true); + // } else { + // dataset.angVelocZ.setValid(false); + // } + // + // dataset.temperature = temperature; + // dataset.temperature.setValid(true); + // } + // break; + // } + 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); + // insertInCommandAndReplyMap(l3gd20h::READ_REGS, 1, &dataset); + // insertInCommandAndReplyMap(l3gd20h::CONFIGURE_CTRL_REGS, 1); + // insertInCommandAndReplyMap(l3gd20h::READ_CTRL_REGS, 1); +} + +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..7d762ad8 --- /dev/null +++ b/mission/devices/GyrL3gCustomHandler.h @@ -0,0 +1,92 @@ +#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; + + std::array cmdBuf; + 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/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..66cf559c --- /dev/null +++ b/mission/devices/devicedefinitions/acsPolling.h @@ -0,0 +1,58 @@ +#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_ +#define MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_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; + // Angular velocities in all axes (X, Y and Z) + int16_t angVelocities[3]; + double accelScaling; + // Accelerations in all axes + int16_t accelerations[3]; + int16_t temperatureRaw; +}; + +struct Adis1650XReply { + bool cfgWasSet = false; + Adis1650XConfig cfg; + bool dataWasSet = false; + Adis1650XData data; +}; + +struct GyroL3gRequest { + SimpleSensorMode mode; + uint8_t ctrlRegs[5]; +}; + +struct GyroL3gReply { + bool cfgWasSet; + uint8_t statusReg; + // Angular velocities in all axes (X, Y and Z) + int16_t angVelocities[3]; + int8_t tempOffsetRaw; + uint8_t ctrlRegs[5]; + float sensitivity; +}; + +} // 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)) { diff --git a/tmtc b/tmtc index 13014eb2..9720fcdd 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 13014eb25053368f4fb9a445788aba71ff98de19 +Subproject commit 9720fcddecb04b228dc5eb0d064f15a12ef8daca From 2f9cdefc967f65de03f388e45b78880d542cf99f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 26 Feb 2023 19:09:12 +0100 Subject: [PATCH 2/9] some more bugfixes --- linux/devices/AcsBoardPolling.cpp | 8 ++++- mission/devices/GyrL3gCustomHandler.cpp | 39 +++---------------------- 2 files changed, 11 insertions(+), 36 deletions(-) diff --git a/linux/devices/AcsBoardPolling.cpp b/linux/devices/AcsBoardPolling.cpp index 9803dfe9..7a5376da 100644 --- a/linux/devices/AcsBoardPolling.cpp +++ b/linux/devices/AcsBoardPolling.cpp @@ -253,11 +253,17 @@ void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) { } 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(), 6); + 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++) { diff --git a/mission/devices/GyrL3gCustomHandler.cpp b/mission/devices/GyrL3gCustomHandler.cpp index 0af32d0c..f1fb31ab 100644 --- a/mission/devices/GyrL3gCustomHandler.cpp +++ b/mission/devices/GyrL3gCustomHandler.cpp @@ -116,11 +116,11 @@ ReturnValue_t GyrL3gCustomHandler::interpretDeviceReply(DeviceCommandId_t id, if (internalState == InternalState::STARTUP) { commandExecuted = true; } - PoolReadGuard readSet(&dataset); + PoolReadGuard pg(&dataset); dataset.setValidity(true, true); - dataset.angVelocX = reply->angVelocities[0]; - dataset.angVelocY = reply->angVelocities[1]; - dataset.angVelocZ = reply->angVelocities[2]; + 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); } @@ -132,34 +132,6 @@ ReturnValue_t GyrL3gCustomHandler::interpretDeviceReply(DeviceCommandId_t id, } dataset.temperature = 25.0 - reply->tempOffsetRaw; } - // PoolReadGuard readSet(&dataset); - // if (readSet.getReadResult() == returnvalue::OK) { - // if (std::abs(angVelocX) < this->absLimitX) { - // dataset.angVelocX = angVelocX; - // dataset.angVelocX.setValid(true); - // } else { - // dataset.angVelocX.setValid(false); - // } - // - // if (std::abs(angVelocY) < this->absLimitY) { - // dataset.angVelocY = angVelocY; - // dataset.angVelocY.setValid(true); - // } else { - // dataset.angVelocY.setValid(false); - // } - // - // if (std::abs(angVelocZ) < this->absLimitZ) { - // dataset.angVelocZ = angVelocZ; - // dataset.angVelocZ.setValid(true); - // } else { - // dataset.angVelocZ.setValid(false); - // } - // - // dataset.temperature = temperature; - // dataset.temperature.setValid(true); - // } - // break; - // } return returnvalue::OK; } @@ -183,9 +155,6 @@ ReturnValue_t GyrL3gCustomHandler::initializeLocalDataPool(localpool::DataPool & void GyrL3gCustomHandler::fillCommandAndReplyMap() { insertInCommandMap(l3gd20h::REQUEST); insertInReplyMap(l3gd20h::REPLY, 5, nullptr, 0, true); - // insertInCommandAndReplyMap(l3gd20h::READ_REGS, 1, &dataset); - // insertInCommandAndReplyMap(l3gd20h::CONFIGURE_CTRL_REGS, 1); - // insertInCommandAndReplyMap(l3gd20h::READ_CTRL_REGS, 1); } void GyrL3gCustomHandler::modeChanged() { internalState = InternalState::NONE; } From 71a2ecc2eaa78d5ba7ca0228407d1683fdcbe5b7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 26 Feb 2023 19:23:55 +0100 Subject: [PATCH 3/9] only perform startup once --- linux/devices/AcsBoardPolling.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/linux/devices/AcsBoardPolling.cpp b/linux/devices/AcsBoardPolling.cpp index 7a5376da..ed245d80 100644 --- a/linux/devices/AcsBoardPolling.cpp +++ b/linux/devices/AcsBoardPolling.cpp @@ -248,6 +248,7 @@ void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) { return; } } + l3g.performStartup = false; l3g.ownReply.cfgWasSet = true; l3g.ownReply.sensitivity = l3gd20h::ctrlReg4ToSensitivity(l3g.sensorCfg[3]); } From cc4c3182a014787e9b3db496ebd8f07b58e6fd4e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 26 Feb 2023 19:38:48 +0100 Subject: [PATCH 4/9] hopefully last bugfix --- linux/devices/AcsBoardPolling.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/linux/devices/AcsBoardPolling.cpp b/linux/devices/AcsBoardPolling.cpp index ed245d80..754fca26 100644 --- a/linux/devices/AcsBoardPolling.cpp +++ b/linux/devices/AcsBoardPolling.cpp @@ -133,6 +133,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send 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; From c7bed10bdfc3c3410b4862126022d1487c496771 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 26 Feb 2023 21:26:49 +0100 Subject: [PATCH 5/9] basic MGM polling done --- bsp_q7s/core/ObjectFactory.cpp | 11 +- dummies/MgmLIS3MDLDummy.cpp | 6 +- dummies/MgmLIS3MDLDummy.h | 5 +- dummies/MgmRm3100Dummy.cpp | 2 +- dummies/MgmRm3100Dummy.h | 5 +- fsfw | 2 +- linux/devices/AcsBoardPolling.cpp | 290 +++++++++++++++++- linux/devices/AcsBoardPolling.h | 46 +-- mission/controller/ThermalController.cpp | 6 +- mission/controller/acs/SensorValues.h | 14 +- .../devices/devicedefinitions/acsPolling.h | 47 ++- 11 files changed, 364 insertions(+), 70 deletions(-) diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index c28806bc..2f21c534 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -349,7 +349,7 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* 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( @@ -365,7 +365,7 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* 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 = @@ -381,9 +381,8 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* #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); @@ -398,7 +397,7 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* 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 = 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/fsfw b/fsfw index cf735143..511d07c0 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit cf735143fe4b79db2fc7faba2b1cd239474e2cfc +Subproject commit 511d07c0c78de7b1850e341dfcf8be7589f3c523 diff --git a/linux/devices/AcsBoardPolling.cpp b/linux/devices/AcsBoardPolling.cpp index 754fca26..1dfdeceb 100644 --- a/linux/devices/AcsBoardPolling.cpp +++ b/linux/devices/AcsBoardPolling.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -34,6 +35,10 @@ ReturnValue_t AcsBoardPolling::performOperation(uint8_t operationCode) { 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); } @@ -49,19 +54,19 @@ ReturnValue_t AcsBoardPolling::initializeInterface(CookieIF* cookie) { } switch (spiCookie->getChipSelectPin()) { case (gpioIds::MGM_0_LIS3_CS): { - mgm0L3Cookie = spiCookie; + mgm0Lis3.cookie = spiCookie; break; } case (gpioIds::MGM_1_RM3100_CS): { - mgm1Rm3100Cookie = spiCookie; + mgm1Rm3100.cookie = spiCookie; break; } case (gpioIds::MGM_2_LIS3_CS): { - mgm2L3Cookie = spiCookie; + mgm2Lis3.cookie = spiCookie; break; } case (gpioIds::MGM_3_RM3100_CS): { - mgm3Rm3100Cookie = spiCookie; + mgm3Rm3100.cookie = spiCookie; break; } case (gpioIds::GYRO_0_ADIS_CS): { @@ -142,7 +147,59 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send } 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.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; @@ -185,28 +242,50 @@ ReturnValue_t AcsBoardPolling::readReceivedMessage(CookieIF* cookie, uint8_t** b 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): { - handleAdisReply(gyro0Adis); - return gyro0Adis.replyResult; + return handleAdisReply(gyro0Adis); } case (gpioIds::GYRO_2_ADIS_CS): { - handleAdisReply(gyro2Adis); - return gyro2Adis.replyResult; + return handleAdisReply(gyro2Adis); } case (gpioIds::GYRO_1_L3G_CS): { - handleL3gReply(gyro1L3g); - return gyro1L3g.replyResult; + return handleL3gReply(gyro1L3g); } case (gpioIds::GYRO_3_L3G_CS): { - handleL3gReply(gyro3L3g); - return gyro3L3g.replyResult; + return handleL3gReply(gyro3L3g); } } return returnvalue::OK; @@ -459,3 +538,190 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) { 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.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 index bacc2a84..2f583028 100644 --- a/linux/devices/AcsBoardPolling.h +++ b/linux/devices/AcsBoardPolling.h @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -25,41 +26,46 @@ class AcsBoardPolling : public SystemObject, std::array cmdBuf; std::array replyBuf; - bool mgm0L3IsOn = false; - SpiCookie* mgm0L3Cookie = nullptr; - bool mgm1Rm3100IsOn = false; - SpiCookie* mgm1Rm3100Cookie = nullptr; - bool mgm2L3IsOn = false; - SpiCookie* mgm2L3Cookie = nullptr; - bool mgm3Rm3100IsOn = false; - SpiCookie* mgm3Rm3100Cookie = nullptr; - - struct GyroAdis { - adis1650x::Type type; - bool isOn = false; - bool performStartup = false; + struct DevBase { SpiCookie* cookie = nullptr; - Countdown countdown; + bool performStartup = false; acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF; ReturnValue_t replyResult; + }; + + struct GyroAdis : public DevBase { + adis1650x::Type type; + Countdown countdown; acs::Adis1650XReply ownReply; acs::Adis1650XReply readerReply; }; GyroAdis gyro0Adis{}; GyroAdis gyro2Adis{}; - struct GyroL3g { - bool performStartup = false; - SpiCookie* cookie = nullptr; - acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF; + struct GyroL3g : public DevBase { uint8_t sensorCfg[5]; - ReturnValue_t replyResult; 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; @@ -74,6 +80,8 @@ class AcsBoardPolling : public SystemObject, 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); }; diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 3097cb5e..92df2453 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include #include #include @@ -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 87a21589..6323b326 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -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/devices/devicedefinitions/acsPolling.h b/mission/devices/devicedefinitions/acsPolling.h index 66cf559c..7398c199 100644 --- a/mission/devices/devicedefinitions/acsPolling.h +++ b/mission/devices/devicedefinitions/acsPolling.h @@ -1,6 +1,7 @@ #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_ #define MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_ +#include "fsfw/thermal/tcsDefinitions.h" #include "gyroAdisHelpers.h" namespace acs { @@ -22,13 +23,13 @@ struct Adis1650XConfig { }; struct Adis1650XData { - double sensitivity; + double sensitivity = 0.0; // Angular velocities in all axes (X, Y and Z) - int16_t angVelocities[3]; - double accelScaling; + int16_t angVelocities[3]{}; + double accelScaling = 0.0; // Accelerations in all axes - int16_t accelerations[3]; - int16_t temperatureRaw; + int16_t accelerations[3]{}; + int16_t temperatureRaw = thermal::INVALID_TEMPERATURE; }; struct Adis1650XReply { @@ -39,18 +40,40 @@ struct Adis1650XReply { }; struct GyroL3gRequest { - SimpleSensorMode mode; - uint8_t ctrlRegs[5]; + SimpleSensorMode mode = SimpleSensorMode::OFF; + uint8_t ctrlRegs[5]{}; }; struct GyroL3gReply { - bool cfgWasSet; + bool cfgWasSet = false; uint8_t statusReg; // Angular velocities in all axes (X, Y and Z) - int16_t angVelocities[3]; - int8_t tempOffsetRaw; - uint8_t ctrlRegs[5]; - float sensitivity; + 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 From e9514b1c973af6425fd63bbc6ab82022c8f8166e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 27 Feb 2023 11:44:52 +0100 Subject: [PATCH 6/9] new MGM polling --- bsp_q7s/core/ObjectFactory.cpp | 14 +- mission/core/pollingSeqTables.cpp | 16 +-- mission/devices/CMakeLists.txt | 2 + mission/devices/GyrAdis1650XHandler.cpp | 20 ++- mission/devices/GyrAdis1650XHandler.h | 2 +- mission/devices/GyrL3gCustomHandler.cpp | 6 +- mission/devices/GyrL3gCustomHandler.h | 1 - mission/devices/MgmLis3CustomHandler.cpp | 146 +++++++++++++++++++++ mission/devices/MgmLis3CustomHandler.h | 105 +++++++++++++++ mission/devices/MgmRm3100CustomHandler.cpp | 127 ++++++++++++++++++ mission/devices/MgmRm3100CustomHandler.h | 98 ++++++++++++++ 11 files changed, 507 insertions(+), 30 deletions(-) create mode 100644 mission/devices/MgmLis3CustomHandler.cpp create mode 100644 mission/devices/MgmLis3CustomHandler.h create mode 100644 mission/devices/MgmRm3100CustomHandler.cpp create mode 100644 mission/devices/MgmRm3100CustomHandler.h diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 2f21c534..6dce14c8 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -5,6 +5,8 @@ #include #include #include +#include +#include #include #include "OBSWConfig.h" @@ -352,7 +354,7 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* new SpiCookie(addresses::MGM_0_LIS3, gpioIds::MGM_0_LIS3_CS, mgmLis3::MAX_BUFFER_SIZE, spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); - auto mgmLis3Handler0 = new MgmLIS3MDLHandler( + auto mgmLis3Handler0 = new MgmLis3CustomHandler( objects::MGM_0_LIS3_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::LIS3_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::MGM_0_LIS3_HANDLER); mgmLis3Handler0->setCustomFdir(fdir); @@ -369,8 +371,8 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); auto mgmRm3100Handler1 = - new MgmRM3100Handler(objects::MGM_1_RM3100_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, - spi::RM3100_TRANSITION_DELAY); + new MgmRm3100CustomHandler(objects::MGM_1_RM3100_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, + spi::RM3100_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::MGM_1_RM3100_HANDLER); mgmRm3100Handler1->setCustomFdir(fdir); assemblyChildren[1] = mgmRm3100Handler1; @@ -384,7 +386,7 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* spiCookie = new SpiCookie(addresses::MGM_2_LIS3, gpioIds::MGM_2_LIS3_CS, mgmLis3::MAX_BUFFER_SIZE, spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); - auto* mgmLis3Handler2 = new MgmLIS3MDLHandler( + auto* mgmLis3Handler2 = new MgmLis3CustomHandler( objects::MGM_2_LIS3_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::LIS3_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::MGM_2_LIS3_HANDLER); mgmLis3Handler2->setCustomFdir(fdir); @@ -401,8 +403,8 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); auto* mgmRm3100Handler3 = - new MgmRM3100Handler(objects::MGM_3_RM3100_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, - spi::RM3100_TRANSITION_DELAY); + new MgmRm3100CustomHandler(objects::MGM_3_RM3100_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, + spi::RM3100_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::MGM_3_RM3100_HANDLER); mgmRm3100Handler3->setCustomFdir(fdir); assemblyChildren[3] = mgmRm3100Handler3; diff --git a/mission/core/pollingSeqTables.cpp b/mission/core/pollingSeqTables.cpp index cdf086fd..6823634e 100644 --- a/mission/core/pollingSeqTables.cpp +++ b/mission/core/pollingSeqTables.cpp @@ -491,9 +491,9 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg DeviceHandlerIF::SEND_WRITE); thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ); thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, @@ -505,9 +505,9 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); + length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ); thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); + length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ); } if (enableBside) { // B side @@ -517,9 +517,9 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg DeviceHandlerIF::SEND_WRITE); thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ); thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, @@ -531,9 +531,9 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); + length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ); thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); + length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ); } if (enableAside) { thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, diff --git a/mission/devices/CMakeLists.txt b/mission/devices/CMakeLists.txt index adcbaf73..cb6c066a 100644 --- a/mission/devices/CMakeLists.txt +++ b/mission/devices/CMakeLists.txt @@ -16,6 +16,8 @@ target_sources( RadiationSensorHandler.cpp GyrAdis1650XHandler.cpp GyrL3gCustomHandler.cpp + MgmRm3100CustomHandler.cpp + MgmLis3CustomHandler.cpp RwHandler.cpp max1227.cpp SusHandler.cpp diff --git a/mission/devices/GyrAdis1650XHandler.cpp b/mission/devices/GyrAdis1650XHandler.cpp index 7a13ec1a..fa28bed5 100644 --- a/mission/devices/GyrAdis1650XHandler.cpp +++ b/mission/devices/GyrAdis1650XHandler.cpp @@ -15,6 +15,10 @@ GyrAdis1650XHandler::GyrAdis1650XHandler(object_id_t objectId, object_id_t devic } void GyrAdis1650XHandler::doStartUp() { + if (internalState != InternalState::STARTUP) { + internalState = InternalState::STARTUP; + commandExecuted = false; + } // Initial 310 ms start up time after power-up if (internalState == InternalState::STARTUP) { if (not commandExecuted) { @@ -29,19 +33,9 @@ void GyrAdis1650XHandler::doStartUp() { } else { setMode(MODE_ON); } + internalState = InternalState::NONE; } } - - // // Read all configuration registers first - // if (internalState == InternalState::CONFIG) { - // if (commandExecuted) { - // commandExecuted = false; - // internalState = InternalState::IDLE; - // } - // } - // - // if (internalState == InternalState::IDLE) { - // } } void GyrAdis1650XHandler::doShutDown() { @@ -50,8 +44,10 @@ void GyrAdis1650XHandler::doShutDown() { primaryDataset.setValidity(false, true); internalState = InternalState::SHUTDOWN; } - if (commandExecuted) { + if (internalState == InternalState::SHUTDOWN and commandExecuted) { updatePeriodicReply(false, adis1650x::REPLY); + internalState = InternalState::NONE; + commandExecuted = false; setMode(_MODE_POWER_DOWN); } } diff --git a/mission/devices/GyrAdis1650XHandler.h b/mission/devices/GyrAdis1650XHandler.h index 3d8e82cf..020dcd6e 100644 --- a/mission/devices/GyrAdis1650XHandler.h +++ b/mission/devices/GyrAdis1650XHandler.h @@ -50,7 +50,7 @@ class GyrAdis1650XHandler : public DeviceHandlerBase { bool goToNormalMode = false; bool warningSwitch = true; - enum class InternalState { STARTUP, SHUTDOWN, IDLE }; + enum class InternalState { NONE, STARTUP, SHUTDOWN }; InternalState internalState = InternalState::STARTUP; bool commandExecuted = false; diff --git a/mission/devices/GyrL3gCustomHandler.cpp b/mission/devices/GyrL3gCustomHandler.cpp index f1fb31ab..40de2650 100644 --- a/mission/devices/GyrL3gCustomHandler.cpp +++ b/mission/devices/GyrL3gCustomHandler.cpp @@ -15,7 +15,7 @@ GyrL3gCustomHandler::GyrL3gCustomHandler(object_id_t objectId, object_id_t devic GyrL3gCustomHandler::~GyrL3gCustomHandler() = default; void GyrL3gCustomHandler::doStartUp() { - if (internalState == InternalState::NONE) { + if (internalState != InternalState::STARTUP) { internalState = InternalState::STARTUP; updatePeriodicReply(true, l3gd20h::REPLY); commandExecuted = false; @@ -28,6 +28,8 @@ void GyrL3gCustomHandler::doStartUp() { } else { setMode(_MODE_TO_ON); } + internalState == InternalState::NONE; + commandExecuted = false; } } } @@ -37,7 +39,7 @@ void GyrL3gCustomHandler::doShutDown() { internalState = InternalState::SHUTDOWN; commandExecuted = false; } - if (commandExecuted) { + if (internalState == InternalState::SHUTDOWN and commandExecuted) { internalState = InternalState::NONE; updatePeriodicReply(false, l3gd20h::REPLY); commandExecuted = false; diff --git a/mission/devices/GyrL3gCustomHandler.h b/mission/devices/GyrL3gCustomHandler.h index 7d762ad8..5f840cfc 100644 --- a/mission/devices/GyrL3gCustomHandler.h +++ b/mission/devices/GyrL3gCustomHandler.h @@ -77,7 +77,6 @@ class GyrL3gCustomHandler : public DeviceHandlerBase { uint8_t ctrlReg4Value = l3gd20h::CTRL_REG_4_VAL; uint8_t ctrlReg5Value = l3gd20h::CTRL_REG_5_VAL; - std::array cmdBuf; acs::GyroL3gRequest gyrRequest{}; // Set default value diff --git a/mission/devices/MgmLis3CustomHandler.cpp b/mission/devices/MgmLis3CustomHandler.cpp new file mode 100644 index 00000000..5c94266a --- /dev/null +++ b/mission/devices/MgmLis3CustomHandler.cpp @@ -0,0 +1,146 @@ +#include + +#include + +#include "fsfw/datapool/PoolReadGuard.h" + +MgmLis3CustomHandler::MgmLis3CustomHandler(object_id_t objectId, object_id_t deviceCommunication, + CookieIF *comCookie, uint32_t transitionDelay) + : DeviceHandlerBase(objectId, deviceCommunication, comCookie), + dataset(this), + transitionDelay(transitionDelay) {} + +MgmLis3CustomHandler::~MgmLis3CustomHandler() = default; + +void MgmLis3CustomHandler::doStartUp() { + if (internalState != InternalState::STARTUP) { + commandExecuted = false; + updatePeriodicReply(true, REPLY); + internalState = InternalState::STARTUP; + } + if (internalState == InternalState::STARTUP) { + if (commandExecuted) { + setMode(MODE_NORMAL); + internalState = InternalState::NONE; + commandExecuted = false; + } + } +} + +void MgmLis3CustomHandler::doShutDown() { + if (internalState != InternalState::SHUTDOWN) { + internalState = InternalState::SHUTDOWN; + commandExecuted = false; + } + if (internalState == InternalState::SHUTDOWN and commandExecuted) { + updatePeriodicReply(false, REPLY); + commandExecuted = false; + internalState = InternalState::NONE; + setMode(_MODE_POWER_DOWN); + } +} + +ReturnValue_t MgmLis3CustomHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + if (internalState == InternalState::STARTUP) { + *id = REQUEST; + return prepareRequest(acs::SimpleSensorMode::NORMAL); + } else if (internalState == InternalState::SHUTDOWN) { + *id = REQUEST; + return prepareRequest(acs::SimpleSensorMode::OFF); + } + return NOTHING_TO_SEND; +} + +ReturnValue_t MgmLis3CustomHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { + *id = REQUEST; + return prepareRequest(acs::SimpleSensorMode::NORMAL); +} + +ReturnValue_t MgmLis3CustomHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, + size_t commandDataLen) { + return NOTHING_TO_SEND; +} + +ReturnValue_t MgmLis3CustomHandler::scanForReply(const uint8_t *start, size_t len, + DeviceCommandId_t *foundId, size_t *foundLen) { + if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) { + return IGNORE_FULL_PACKET; + } + if (len != sizeof(acs::MgmRm3100Reply)) { + *foundLen = len; + return returnvalue::FAILED; + } + *foundId = REPLY; + *foundLen = len; + return returnvalue::OK; +} +ReturnValue_t MgmLis3CustomHandler::interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) { + const auto *reply = reinterpret_cast(packet); + if (reply->dataWasSet) { + if (internalState == InternalState::STARTUP) { + commandExecuted = true; + } + PoolReadGuard pg(&dataset); + for (uint8_t idx = 0; idx < 3; idx++) { + dataset.fieldStrengths[idx] = + reply->mgmValuesRaw[idx] * reply->sensitivity * mgmLis3::GAUSS_TO_MICROTESLA_FACTOR; + } + + dataset.setValidity(true, true); + if (std::abs(dataset.fieldStrengths[0]) > absLimitX or + std::abs(dataset.fieldStrengths[1]) > absLimitY or + std::abs(dataset.fieldStrengths[2]) > absLimitZ) { + dataset.fieldStrengths.setValid(false); + } + dataset.temperature = 25.0 + ((static_cast(reply->temperatureRaw)) / 8.0); + } + return returnvalue::OK; +} + +void MgmLis3CustomHandler::fillCommandAndReplyMap() { + insertInCommandMap(REQUEST); + insertInReplyMap(REPLY, 5, nullptr, 0, true); +} + +void MgmLis3CustomHandler::setToGoToNormalMode(bool enable) { this->goToNormalMode = enable; } + +uint32_t MgmLis3CustomHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { + return transitionDelay; +} + +void MgmLis3CustomHandler::modeChanged(void) { internalState = InternalState::NONE; } + +ReturnValue_t MgmLis3CustomHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(mgmLis3::FIELD_STRENGTHS, &mgmXYZ); + localDataPoolMap.emplace(mgmLis3::TEMPERATURE_CELCIUS, &temperature); + poolManager.subscribeForRegularPeriodicPacket({dataset.getSid(), false, 10.0}); + return returnvalue::OK; +} + +void MgmLis3CustomHandler::setAbsoluteLimits(float xLimit, float yLimit, float zLimit) { + this->absLimitX = xLimit; + this->absLimitY = yLimit; + this->absLimitZ = zLimit; +} + +void MgmLis3CustomHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) { + periodicPrintout = enable; + debugDivider.setDivider(divider); +} + +ReturnValue_t MgmLis3CustomHandler::prepareRequest(acs::SimpleSensorMode mode) { + request.mode = mode; + rawPacket = reinterpret_cast(&request); + rawPacketLen = sizeof(acs::MgmLis3Request); + return returnvalue::OK; +} + +LocalPoolDataSetBase *MgmLis3CustomHandler::getDataSetHandle(sid_t sid) { + if (sid == dataset.getSid()) { + return &dataset; + } + return nullptr; +} diff --git a/mission/devices/MgmLis3CustomHandler.h b/mission/devices/MgmLis3CustomHandler.h new file mode 100644 index 00000000..58e7e182 --- /dev/null +++ b/mission/devices/MgmLis3CustomHandler.h @@ -0,0 +1,105 @@ +#ifndef MISSION_DEVICES_MGMLIS3CUSTOMHANDLER_H_ +#define MISSION_DEVICES_MGMLIS3CUSTOMHANDLER_H_ + +#include + +#include "fsfw/devicehandlers/DeviceHandlerBase.h" +#include "fsfw/globalfunctions/PeriodicOperationDivider.h" +#include "mission/devices/devicedefinitions/acsPolling.h" + +class PeriodicOperationDivider; + +/** + * @brief Device handler object for the LIS3MDL 3-axis magnetometer + * by STMicroeletronics + * @details + * Datasheet can be found online by googling LIS3MDL. + * Flight manual: + * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/LIS3MDL_MGM + * @author L. Loidold, R. Mueller + */ +class MgmLis3CustomHandler : public DeviceHandlerBase { + public: + static constexpr DeviceCommandId_t REQUEST = 0x70; + static constexpr DeviceCommandId_t REPLY = 0x77; + + static const uint8_t INTERFACE_ID = CLASS_ID::MGM_LIS3MDL; + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::MGM_LIS3MDL; + // Notifies a command to change the setup parameters + static const Event CHANGE_OF_SETUP_PARAMETER = MAKE_EVENT(0, severity::LOW); + + MgmLis3CustomHandler(uint32_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, + uint32_t transitionDelay); + virtual ~MgmLis3CustomHandler(); + + void enablePeriodicPrintouts(bool enable, uint8_t divider); + /** + * Set the absolute limit for the values on the axis in microtesla. The dataset values will + * be marked as invalid if that limit is exceeded + * @param xLimit + * @param yLimit + * @param zLimit + */ + void setAbsoluteLimits(float xLimit, float yLimit, float zLimit); + void setToGoToNormalMode(bool enable); + + protected: + /** DeviceHandlerBase overrides */ + void doShutDown() override; + void doStartUp() override; + virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) override; + ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, + size_t *foundLen) override; + /** + * This implementation is tailored towards space applications and will flag values larger + * than 100 microtesla on X,Y and 150 microtesla on Z as invalid + * @param id + * @param packet + * @return + */ + virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; + void fillCommandAndReplyMap() override; + void modeChanged(void) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; + LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override; + + private: + mgmLis3::MgmPrimaryDataset dataset; + acs::MgmLis3Request request; + + uint32_t transitionDelay; + + float absLimitX = 100; + float absLimitY = 100; + float absLimitZ = 150; + + uint8_t statusRegister = 0; + bool goToNormalMode = false; + + enum class InternalState { + NONE, + STARTUP, + SHUTDOWN, + }; + + InternalState internalState = InternalState::NONE; + bool commandExecuted = false; + + PoolEntry mgmXYZ = PoolEntry(3); + PoolEntry temperature = PoolEntry(); + /*------------------------------------------------------------------------*/ + /* Device specific commands and variables */ + /*------------------------------------------------------------------------*/ + + bool periodicPrintout = false; + PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3); + + ReturnValue_t prepareRequest(acs::SimpleSensorMode mode); +}; + +#endif /* MISSION_DEVICES_MGMLIS3CUSTOMHANDLER_H_ */ diff --git a/mission/devices/MgmRm3100CustomHandler.cpp b/mission/devices/MgmRm3100CustomHandler.cpp new file mode 100644 index 00000000..4c40bf29 --- /dev/null +++ b/mission/devices/MgmRm3100CustomHandler.cpp @@ -0,0 +1,127 @@ +#include + +#include "fsfw/datapool/PoolReadGuard.h" +#include "fsfw/devicehandlers/DeviceHandlerMessage.h" +#include "fsfw/globalfunctions/bitutility.h" +#include "fsfw/objectmanager/SystemObjectIF.h" +#include "fsfw/returnvalues/returnvalue.h" + +MgmRm3100CustomHandler::MgmRm3100CustomHandler(object_id_t objectId, + object_id_t deviceCommunication, CookieIF *comCookie, + uint32_t transitionDelay) + : DeviceHandlerBase(objectId, deviceCommunication, comCookie), + primaryDataset(this), + transitionDelay(transitionDelay) {} + +MgmRm3100CustomHandler::~MgmRm3100CustomHandler() = default; + +void MgmRm3100CustomHandler::doStartUp() { + if (internalState != InternalState::STARTUP) { + commandExecuted = false; + updatePeriodicReply(true, REPLY); + internalState = InternalState::STARTUP; + } + if (internalState == InternalState::STARTUP) { + if (commandExecuted) { + commandExecuted = false; + internalState = InternalState::NONE; + setMode(MODE_NORMAL); + } + } +} + +void MgmRm3100CustomHandler::doShutDown() { + if (internalState != InternalState::SHUTDOWN) { + commandExecuted = false; + internalState = InternalState::SHUTDOWN; + } + if (internalState == InternalState::SHUTDOWN and commandExecuted) { + updatePeriodicReply(false, REPLY); + setMode(_MODE_POWER_DOWN); + commandExecuted = false; + } +} + +ReturnValue_t MgmRm3100CustomHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + if (internalState == InternalState::STARTUP) { + *id = REQUEST; + return prepareRequest(acs::SimpleSensorMode::NORMAL); + } else if (internalState == InternalState::SHUTDOWN) { + *id = REQUEST; + return prepareRequest(acs::SimpleSensorMode::OFF); + } + return NOTHING_TO_SEND; +} + +ReturnValue_t MgmRm3100CustomHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, + size_t commandDataLen) { + return NOTHING_TO_SEND; +} + +ReturnValue_t MgmRm3100CustomHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { + *id = REQUEST; + return prepareRequest(acs::SimpleSensorMode::NORMAL); +} + +ReturnValue_t MgmRm3100CustomHandler::scanForReply(const uint8_t *start, size_t len, + DeviceCommandId_t *foundId, size_t *foundLen) { + if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) { + return IGNORE_FULL_PACKET; + } + if (len != sizeof(acs::MgmRm3100Reply)) { + *foundLen = len; + return returnvalue::FAILED; + } + *foundId = REPLY; + *foundLen = len; + return returnvalue::OK; +} + +ReturnValue_t MgmRm3100CustomHandler::interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) { + const auto *reply = reinterpret_cast(packet); + if (reply->dataWasRead) { + if (internalState == InternalState::STARTUP) { + commandExecuted = true; + } + + PoolReadGuard pg(&primaryDataset); + for (uint8_t idx = 0; idx < 3; idx++) { + primaryDataset.fieldStrengths[idx] = reply->mgmValuesRaw[idx] * reply->scaleFactors[idx]; + } + } + return returnvalue::OK; +} + +void MgmRm3100CustomHandler::fillCommandAndReplyMap() { + insertInCommandMap(REQUEST); + insertInReplyMap(REPLY, 5, nullptr, 0, true); +} + +void MgmRm3100CustomHandler::modeChanged() { internalState = InternalState::NONE; } + +ReturnValue_t MgmRm3100CustomHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(mgmRm3100::FIELD_STRENGTHS, &mgmXYZ); + poolManager.subscribeForRegularPeriodicPacket({primaryDataset.getSid(), false, 10.0}); + return returnvalue::OK; +} + +uint32_t MgmRm3100CustomHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { + return this->transitionDelay; +} + +void MgmRm3100CustomHandler::setToGoToNormalMode(bool enable) { goToNormalModeAtStartup = enable; } + +void MgmRm3100CustomHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) { + periodicPrintout = enable; + debugDivider.setDivider(divider); +} + +ReturnValue_t MgmRm3100CustomHandler::prepareRequest(acs::SimpleSensorMode mode) { + request.mode = mode; + rawPacket = reinterpret_cast(&request); + rawPacketLen = sizeof(acs::MgmRm3100Request); + return returnvalue::OK; +} diff --git a/mission/devices/MgmRm3100CustomHandler.h b/mission/devices/MgmRm3100CustomHandler.h new file mode 100644 index 00000000..c4821cc7 --- /dev/null +++ b/mission/devices/MgmRm3100CustomHandler.h @@ -0,0 +1,98 @@ +#ifndef MISSION_DEVICES_MGMRM3100CUSTOMHANDLER_H_ +#define MISSION_DEVICES_MGMRM3100CUSTOMHANDLER_H_ + +#include + +#include "fsfw/devicehandlers/DeviceHandlerBase.h" +#include "fsfw/globalfunctions/PeriodicOperationDivider.h" +#include "mission/devices/devicedefinitions/acsPolling.h" + +/** + * @brief Device Handler for the RM3100 geomagnetic magnetometer sensor + * (https://www.pnicorp.com/rm3100/) + * @details + * Flight manual: + * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/RM3100_MGM + */ +class MgmRm3100CustomHandler : public DeviceHandlerBase { + public: + static constexpr DeviceCommandId_t REQUEST = 0x70; + static constexpr DeviceCommandId_t REPLY = 0x77; + + static const uint8_t INTERFACE_ID = CLASS_ID::MGM_RM3100; + + //! [EXPORT] : [COMMENT] P1: TMRC value which was set, P2: 0 + static constexpr Event TMRC_SET = + event::makeEvent(SUBSYSTEM_ID::MGM_RM3100, 0x00, severity::INFO); + + //! [EXPORT] : [COMMENT] Cycle counter set. P1: First two bytes new Cycle Count X + //! P1: Second two bytes new Cycle Count Y + //! P2: New cycle count Z + static constexpr Event cycleCountersSet = + event::makeEvent(SUBSYSTEM_ID::MGM_RM3100, 0x01, severity::INFO); + + MgmRm3100CustomHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, + uint32_t transitionDelay); + virtual ~MgmRm3100CustomHandler(); + + void enablePeriodicPrintouts(bool enable, uint8_t divider); + /** + * Configure device handler to go to normal mode after startup immediately + * @param enable + */ + void setToGoToNormalMode(bool enable); + + protected: + /* DeviceHandlerBase overrides */ + ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override; + void doStartUp() override; + void doShutDown() override; + ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) override; + ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, + size_t *foundLen) override; + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; + + void fillCommandAndReplyMap() override; + void modeChanged(void) override; + virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; + + private: + enum class InternalState { NONE, STARTUP, SHUTDOWN }; + InternalState internalState = InternalState::NONE; + bool commandExecuted = false; + mgmRm3100::Rm3100PrimaryDataset primaryDataset; + acs::MgmRm3100Request request{}; + + // uint8_t cmmRegValue = mgmRm3100::CMM_VALUE; + // uint8_t tmrcRegValue = mgmRm3100::TMRC_DEFAULT_VALUE; + // uint16_t cycleCountRegValueX = mgmRm3100::CYCLE_COUNT_VALUE; + // uint16_t cycleCountRegValueY = mgmRm3100::CYCLE_COUNT_VALUE; + // uint16_t cycleCountRegValueZ = mgmRm3100::CYCLE_COUNT_VALUE; + float scaleFactorX = 1.0 / mgmRm3100::DEFAULT_GAIN; + float scaleFactorY = 1.0 / mgmRm3100::DEFAULT_GAIN; + float scaleFactorZ = 1.0 / mgmRm3100::DEFAULT_GAIN; + + bool goToNormalModeAtStartup = false; + uint32_t transitionDelay; + PoolEntry mgmXYZ = PoolEntry(3); + + ReturnValue_t handleCycleCountConfigCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, size_t commandDataLen); + ReturnValue_t handleCycleCommand(bool oneCycleValue, const uint8_t *commandData, + size_t commandDataLen); + + ReturnValue_t handleTmrcConfigCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen); + + // ReturnValue_t handleDataReadout(const uint8_t *packet); + + bool periodicPrintout = false; + ReturnValue_t prepareRequest(acs::SimpleSensorMode mode); + PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3); +}; + +#endif /* MISSION_DEVICEHANDLING_MgmRm3100CustomHandler_H_ */ From d4923ac3e87cd15a414dfddab7e0591b873e2a2c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Feb 2023 01:25:25 +0100 Subject: [PATCH 7/9] =?UTF-8?q?w=C3=B6rks?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- bsp_q7s/core/ObjectFactory.cpp | 18 +- linux/devices/AcsBoardPolling.cpp | 3 + linux/devices/AcsBoardPolling.h | 2 +- mission/devices/GyrAdis1650XHandler.cpp | 294 +-------------------- mission/devices/GyrL3gCustomHandler.cpp | 6 +- mission/devices/MgmLis3CustomHandler.cpp | 6 +- mission/devices/MgmLis3CustomHandler.h | 2 +- mission/devices/MgmRm3100CustomHandler.cpp | 11 + mission/devices/MgmRm3100CustomHandler.h | 5 +- tmtc | 2 +- 10 files changed, 43 insertions(+), 306 deletions(-) diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 6dce14c8..93bec365 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -354,8 +354,9 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* new SpiCookie(addresses::MGM_0_LIS3, gpioIds::MGM_0_LIS3_CS, mgmLis3::MAX_BUFFER_SIZE, spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); - auto mgmLis3Handler0 = new MgmLis3CustomHandler( - 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; @@ -371,8 +372,8 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); auto mgmRm3100Handler1 = - new MgmRm3100CustomHandler(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; @@ -386,8 +387,9 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* spiCookie = new SpiCookie(addresses::MGM_2_LIS3, gpioIds::MGM_2_LIS3_CS, mgmLis3::MAX_BUFFER_SIZE, spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); - auto* mgmLis3Handler2 = new MgmLis3CustomHandler( - 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; @@ -403,8 +405,8 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); auto* mgmRm3100Handler3 = - new MgmRm3100CustomHandler(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; diff --git a/linux/devices/AcsBoardPolling.cpp b/linux/devices/AcsBoardPolling.cpp index 1dfdeceb..871cfdd0 100644 --- a/linux/devices/AcsBoardPolling.cpp +++ b/linux/devices/AcsBoardPolling.cpp @@ -178,6 +178,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send if (req->mode == acs::SimpleSensorMode::NORMAL) { mgm.performStartup = true; } else { + mgm.ownReply.dataWasRead = false; } mgm.mode = req->mode; } @@ -717,6 +718,8 @@ void AcsBoardPolling::mgmRm3100Handler(MgmRm3100& mgm) { // 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] = diff --git a/linux/devices/AcsBoardPolling.h b/linux/devices/AcsBoardPolling.h index 2f583028..58c35786 100644 --- a/linux/devices/AcsBoardPolling.h +++ b/linux/devices/AcsBoardPolling.h @@ -30,7 +30,7 @@ class AcsBoardPolling : public SystemObject, SpiCookie* cookie = nullptr; bool performStartup = false; acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF; - ReturnValue_t replyResult; + ReturnValue_t replyResult = returnvalue::OK; }; struct GyroAdis : public DevBase { diff --git a/mission/devices/GyrAdis1650XHandler.cpp b/mission/devices/GyrAdis1650XHandler.cpp index fa28bed5..1a91d36f 100644 --- a/mission/devices/GyrAdis1650XHandler.cpp +++ b/mission/devices/GyrAdis1650XHandler.cpp @@ -71,11 +71,7 @@ ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_ return returnvalue::OK; } default: { - // Might be a configuration error - sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: " - "Unknown internal state!" - << std::endl; - return returnvalue::OK; + return NOTHING_TO_SEND; } } return returnvalue::OK; @@ -84,106 +80,12 @@ ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_ ReturnValue_t GyrAdis1650XHandler::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; - // adis1650x::prepareReadCommand(regList, sizeof(regList), commandBuffer.data(), - // commandBuffer.size()); - // 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; - // } - // TODO: Convert to special request - - // 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; return NOTHING_TO_SEND; } void GyrAdis1650XHandler::fillCommandAndReplyMap() { insertInCommandMap(adis1650x::REQUEST); insertInReplyMap(adis1650x::REPLY, 5, nullptr, 0, true); - // 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 GyrAdis1650XHandler::scanForReply(const uint8_t *start, size_t remainingSize, @@ -197,7 +99,9 @@ ReturnValue_t GyrAdis1650XHandler::scanForReply(const uint8_t *start, size_t rem } *foundId = adis1650x::REPLY; *foundLen = remainingSize; - + if (internalState == InternalState::SHUTDOWN) { + commandExecuted = true; + } return returnvalue::OK; } @@ -263,94 +167,6 @@ LocalPoolDataSetBase *GyrAdis1650XHandler::getDataSetHandle(sid_t sid) { return nullptr; } -// ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) { -// using namespace adis1650x; -// 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 (adis1650x::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 GyrAdis1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 6000; } void GyrAdis1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne, @@ -394,108 +210,6 @@ adis1650x::BurstModes GyrAdis1650XHandler::getBurstMode() { return adis1650x::burstModeFromMscCtrl(currentCtrlReg); } -//#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 GyrAdis1650XHandler::setToGoToNormalModeImmediately() { goToNormalMode = true; } void GyrAdis1650XHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) { diff --git a/mission/devices/GyrL3gCustomHandler.cpp b/mission/devices/GyrL3gCustomHandler.cpp index 40de2650..4bd5069f 100644 --- a/mission/devices/GyrL3gCustomHandler.cpp +++ b/mission/devices/GyrL3gCustomHandler.cpp @@ -28,7 +28,7 @@ void GyrL3gCustomHandler::doStartUp() { } else { setMode(_MODE_TO_ON); } - internalState == InternalState::NONE; + internalState = InternalState::NONE; commandExecuted = false; } } @@ -37,6 +37,7 @@ void GyrL3gCustomHandler::doStartUp() { void GyrL3gCustomHandler::doShutDown() { if (internalState != InternalState::SHUTDOWN) { internalState = InternalState::SHUTDOWN; + dataset.setValidity(false, true); commandExecuted = false; } if (internalState == InternalState::SHUTDOWN and commandExecuted) { @@ -108,6 +109,9 @@ ReturnValue_t GyrL3gCustomHandler::scanForReply(const uint8_t *start, size_t len } *foundId = l3gd20h::REPLY; *foundLen = len; + if (internalState == InternalState::SHUTDOWN) { + commandExecuted = true; + } return returnvalue::OK; } diff --git a/mission/devices/MgmLis3CustomHandler.cpp b/mission/devices/MgmLis3CustomHandler.cpp index 5c94266a..7968abd7 100644 --- a/mission/devices/MgmLis3CustomHandler.cpp +++ b/mission/devices/MgmLis3CustomHandler.cpp @@ -29,6 +29,7 @@ void MgmLis3CustomHandler::doStartUp() { void MgmLis3CustomHandler::doShutDown() { if (internalState != InternalState::SHUTDOWN) { + dataset.setValidity(false, true); internalState = InternalState::SHUTDOWN; commandExecuted = false; } @@ -67,12 +68,15 @@ ReturnValue_t MgmLis3CustomHandler::scanForReply(const uint8_t *start, size_t le if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) { return IGNORE_FULL_PACKET; } - if (len != sizeof(acs::MgmRm3100Reply)) { + 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, diff --git a/mission/devices/MgmLis3CustomHandler.h b/mission/devices/MgmLis3CustomHandler.h index 58e7e182..621d81e4 100644 --- a/mission/devices/MgmLis3CustomHandler.h +++ b/mission/devices/MgmLis3CustomHandler.h @@ -70,7 +70,7 @@ class MgmLis3CustomHandler : public DeviceHandlerBase { private: mgmLis3::MgmPrimaryDataset dataset; - acs::MgmLis3Request request; + acs::MgmLis3Request request{}; uint32_t transitionDelay; diff --git a/mission/devices/MgmRm3100CustomHandler.cpp b/mission/devices/MgmRm3100CustomHandler.cpp index 4c40bf29..685de23d 100644 --- a/mission/devices/MgmRm3100CustomHandler.cpp +++ b/mission/devices/MgmRm3100CustomHandler.cpp @@ -33,6 +33,7 @@ void MgmRm3100CustomHandler::doStartUp() { void MgmRm3100CustomHandler::doShutDown() { if (internalState != InternalState::SHUTDOWN) { commandExecuted = false; + primaryDataset.setValidity(false, true); internalState = InternalState::SHUTDOWN; } if (internalState == InternalState::SHUTDOWN and commandExecuted) { @@ -75,6 +76,9 @@ ReturnValue_t MgmRm3100CustomHandler::scanForReply(const uint8_t *start, size_t } *foundId = REPLY; *foundLen = len; + if (internalState == InternalState::SHUTDOWN) { + commandExecuted = true; + } return returnvalue::OK; } @@ -125,3 +129,10 @@ ReturnValue_t MgmRm3100CustomHandler::prepareRequest(acs::SimpleSensorMode mode) 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 index c4821cc7..4c0c98b3 100644 --- a/mission/devices/MgmRm3100CustomHandler.h +++ b/mission/devices/MgmRm3100CustomHandler.h @@ -59,6 +59,7 @@ class MgmRm3100CustomHandler : public DeviceHandlerBase { 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 }; @@ -79,6 +80,7 @@ class MgmRm3100CustomHandler : public DeviceHandlerBase { 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); @@ -88,9 +90,6 @@ class MgmRm3100CustomHandler : public DeviceHandlerBase { ReturnValue_t handleTmrcConfigCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen); - // ReturnValue_t handleDataReadout(const uint8_t *packet); - - bool periodicPrintout = false; ReturnValue_t prepareRequest(acs::SimpleSensorMode mode); PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3); }; diff --git a/tmtc b/tmtc index 9720fcdd..0ce32521 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 9720fcddecb04b228dc5eb0d064f15a12ef8daca +Subproject commit 0ce32521f44bc4dea22c7d2e74099ff9fa2ed610 From 10807d577f4ce1669abfb804e28768f7eb3a038b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Feb 2023 11:53:34 +0100 Subject: [PATCH 8/9] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 0ce32521..481e57be 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 0ce32521f44bc4dea22c7d2e74099ff9fa2ed610 +Subproject commit 481e57be5919565fa6be9cdb28e3a454dad707cc From f2598b5c4c911b4511895c662cfc7836799d376c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Feb 2023 15:02:37 +0100 Subject: [PATCH 9/9] small fix for sched blocks --- mission/core/pollingSeqTables.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/core/pollingSeqTables.cpp b/mission/core/pollingSeqTables.cpp index 6823634e..7ceab326 100644 --- a/mission/core/pollingSeqTables.cpp +++ b/mission/core/pollingSeqTables.cpp @@ -553,7 +553,7 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg 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_3_PERIOD, + 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);