From c7bed10bdfc3c3410b4862126022d1487c496771 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 26 Feb 2023 21:26:49 +0100 Subject: [PATCH] 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