From 047af1bdceb48574695872b7ca2c9d2bc7436a16 Mon Sep 17 00:00:00 2001 From: Fabrizio Monti <fabrizio.monti@skywarder.eu> Date: Mon, 24 Mar 2025 14:57:26 +0100 Subject: [PATCH] [ENGINE] Updated sensors for Orion. --- .vscode/c_cpp_properties.json | 22 ++++ CMakeLists.txt | 2 +- src/Motor/Sensors/Sensors.cpp | 194 +++++++++++++++++++++++++------- src/Motor/Sensors/Sensors.h | 35 ++++-- src/Motor/Sensors/SensorsData.h | 22 +++- 5 files changed, 216 insertions(+), 59 deletions(-) diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index f27148483..8904aa986 100755 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -487,6 +487,28 @@ "${workspaceFolder}/skyward-boardcore/src/bsps/stm32f767zi_orion_biscotto" ] }, + { + "name": "stm32f767zi_orion_engine", + "cStandard": "c11", + "cppStandard": "c++14", + "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++", + "defines": [ + "${defaultDefines}", + "_MIOSIX_BOARDNAME=stm32f767zi_orion_engine", + "_BOARD_STM32F767ZI_ORION_ENGINE", + "_ARCH_CORTEXM7_STM32F7", + "HSE_VALUE=25000000", + "SYSCLK_FREQ_216MHz=216000000", + "__ENABLE_XRAM", + "V_DDA_VOLTAGE=3.3f" + ], + "includePath": [ + "${defaultIncludePaths}", + "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/common", + "${workspaceFolder}/skyward-boardcore/src/bsps/stm32f767zi_orion_engine/config", + "${workspaceFolder}/skyward-boardcore/src/bsps/stm32f767zi_orion_engine" + ] + }, { "name": "logdecoder", "includePath": [ diff --git a/CMakeLists.txt b/CMakeLists.txt index 1902a11e2..34f5723f2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,7 +66,7 @@ sbs_target(payload-entry-euroc stm32f767zi_lyra_biscotto) add_executable(motor-entry src/Motor/motor-entry.cpp ${MOTOR_SOURCES}) target_include_directories(motor-entry PRIVATE ${OBSW_INCLUDE_DIRS}) -sbs_target(motor-entry stm32f767zi_lyra_motor) +sbs_target(motor-entry stm32f767zi_orion_engine) add_executable(rig-v2-entry src/RIGv2/rig-v2-entry.cpp ${RIG_V2_COMPUTER}) target_include_directories(rig-v2-entry PRIVATE ${OBSW_INCLUDE_DIRS}) diff --git a/src/Motor/Sensors/Sensors.cpp b/src/Motor/Sensors/Sensors.cpp index c930178ca..06211420c 100644 --- a/src/Motor/Sensors/Sensors.cpp +++ b/src/Motor/Sensors/Sensors.cpp @@ -48,8 +48,9 @@ bool Sensors::start() if (Config::Sensors::ADS131M08::ENABLED) { ads131m08Init(); - topTankPressureInit(); - bottomTankPressureInit(); + oxTopTankPressureInit(); + oxBottomTankPressureInit(); + n2TankPressureInit(); ccPressureInit(); tankTempInit(); } @@ -97,20 +98,37 @@ LIS2MDLData Sensors::getLIS2MDLLastSample() return lis2mdl ? lis2mdl->getLastSample() : LIS2MDLData{}; } -LSM6DSRXData Sensors::getLSM6DSRXLastSample() +LSM6DSRXData Sensors::getLSM6DSRX0LastSample() { - return lsm6dsrx ? lsm6dsrx->getLastSample() : LSM6DSRXData{}; + return lsm6dsrx0 ? lsm6dsrx0->getLastSample() : LSM6DSRXData{}; } -PressureData Sensors::getTopTankPressLastSample() +LSM6DSRXData Sensors::getLSM6DSRX1LastSample() { - return topTankPressure ? topTankPressure->getLastSample() : PressureData{}; + return lsm6dsrx1 ? lsm6dsrx1->getLastSample() : LSM6DSRXData{}; } -PressureData Sensors::getBottomTankPressLastSample() +PressureData Sensors::getOxTopTankPressLastSample() { - return bottomTankPressure ? bottomTankPressure->getLastSample() - : PressureData{}; + return oxTopTankPressure ? oxTopTankPressure->getLastSample() + : PressureData{}; +} + +PressureData Sensors::getOxBottomTankPress0LastSample() +{ + return oxBottomTankPressure0 ? oxBottomTankPressure0->getLastSample() + : PressureData{}; +} + +PressureData Sensors::getOxBottomTankPress1LastSample() +{ + return oxBottomTankPressure1 ? oxBottomTankPressure1->getLastSample() + : PressureData{}; +} + +PressureData Sensors::getN2TankPressLastSample() +{ + return n2TankPressure ? n2TankPressure->getLastSample() : PressureData{}; } PressureData Sensors::getCCPressLastSample() @@ -146,8 +164,11 @@ std::vector<SensorInfo> Sensors::getSensorInfos() if (lis2mdl) infos.push_back(manager->getSensorInfo(lis2mdl.get())); - if (lsm6dsrx) - infos.push_back(manager->getSensorInfo(lsm6dsrx.get())); + if (lsm6dsrx0) + infos.push_back(manager->getSensorInfo(lsm6dsrx0.get())); + + if (lsm6dsrx1) + infos.push_back(manager->getSensorInfo(lsm6dsrx1.get())); if (ads131m08) infos.push_back(manager->getSensorInfo(ads131m08.get())); @@ -155,11 +176,19 @@ std::vector<SensorInfo> Sensors::getSensorInfos() if (internalAdc) infos.push_back(manager->getSensorInfo(internalAdc.get())); - if (topTankPressure) - infos.push_back(manager->getSensorInfo(topTankPressure.get())); + if (oxTopTankPressure) + infos.push_back(manager->getSensorInfo(oxTopTankPressure.get())); + + if (oxBottomTankPressure0) + infos.push_back( + manager->getSensorInfo(oxBottomTankPressure0.get())); + + if (oxBottomTankPressure1) + infos.push_back( + manager->getSensorInfo(oxBottomTankPressure1.get())); - if (bottomTankPressure) - infos.push_back(manager->getSensorInfo(bottomTankPressure.get())); + if (n2TankPressure) + infos.push_back(manager->getSensorInfo(n2TankPressure.get())); if (ccPressure) infos.push_back(manager->getSensorInfo(ccPressure.get())); @@ -249,18 +278,34 @@ void Sensors::lsm6dsrxInit() LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1; config.fifoTemperatureBdr = LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED; - lsm6dsrx = std::make_unique<LSM6DSRX>(getModule<Buses>()->getLSM6DSRX(), - sensors::LSM6DSRX::cs::getPin(), - spiConfig, config); + lsm6dsrx0 = std::make_unique<LSM6DSRX>(getModule<Buses>()->getLSM6DSRX(), + sensors::LSM6DSRX0::cs::getPin(), + spiConfig, config); + lsm6dsrx1 = std::make_unique<LSM6DSRX>(getModule<Buses>()->getLSM6DSRX(), + sensors::LSM6DSRX1::cs::getPin(), + spiConfig, config); } -void Sensors::lsm6dsrxCallback() +void Sensors::lsm6dsrx0Callback() { - if (!lsm6dsrx) + if (!lsm6dsrx0) return; uint16_t lastFifoSize; - const auto lastFifo = lsm6dsrx->getLastFifo(lastFifoSize); + const auto lastFifo = lsm6dsrx0->getLastFifo(lastFifoSize); + + // For every instance inside the fifo log the sample + for (uint16_t i = 0; i < lastFifoSize; i++) + sdLogger.log(lastFifo.at(i)); +} + +void Sensors::lsm6dsrx1Callback() +{ + if (!lsm6dsrx1) + return; + + uint16_t lastFifoSize; + const auto lastFifo = lsm6dsrx1->getLastFifo(lastFifoSize); // For every instance inside the fifo log the sample for (uint16_t i = 0; i < lastFifoSize; i++) @@ -335,9 +380,9 @@ void Sensors::internalAdcCallback() sdLogger.log(getInternalADCLastSample()); } -void Sensors::topTankPressureInit() +void Sensors::oxTopTankPressureInit() { - topTankPressure = std::make_unique<TrafagPressureSensor>( + oxTopTankPressure = std::make_unique<TrafagPressureSensor>( [this]() { auto sample = getADS131M08LastSample(); @@ -348,31 +393,75 @@ void Sensors::topTankPressureInit() Config::Sensors::Trafag::TANK_TOP_MAX_PRESSURE, Config::Sensors::Trafag::MIN_CURRENT, Config::Sensors::Trafag::MAX_CURRENT); + // TODO: is configuration correct? } -void Sensors::topTankPressureCallback() +void Sensors::oxTopTankPressureCallback() { - sdLogger.log(TopTankPressureData{getTopTankPressLastSample()}); + sdLogger.log(OxTopTankPressureData{getOxTopTankPressLastSample()}); } -void Sensors::bottomTankPressureInit() +void Sensors::oxBottomTankPressureInit() { - bottomTankPressure = std::make_unique<TrafagPressureSensor>( + oxBottomTankPressure0 = std::make_unique<TrafagPressureSensor>( [this]() { auto sample = getADS131M08LastSample(); return sample.getVoltage( Config::Sensors::ADS131M08::TANK_BOTTOM_PT_CHANNEL); + // TODO: which is the correct channel? }, Config::Sensors::Trafag::TANK_BOTTOM_SHUNT_RESISTANCE, Config::Sensors::Trafag::TANK_BOTTOM_MAX_PRESSURE, Config::Sensors::Trafag::MIN_CURRENT, Config::Sensors::Trafag::MAX_CURRENT); + // TODO: is this configuration correct? + + oxBottomTankPressure1 = std::make_unique<TrafagPressureSensor>( + [this]() + { + auto sample = getADS131M08LastSample(); + return sample.getVoltage( + Config::Sensors::ADS131M08::TANK_BOTTOM_PT_CHANNEL); + // TODO: which is the correct channel? + }, + Config::Sensors::Trafag::TANK_BOTTOM_SHUNT_RESISTANCE, + Config::Sensors::Trafag::TANK_BOTTOM_MAX_PRESSURE, + Config::Sensors::Trafag::MIN_CURRENT, + Config::Sensors::Trafag::MAX_CURRENT); + // TODO: is this configuration correct? +} + +void Sensors::oxBottomTankPressure0Callback() +{ + sdLogger.log(OxBottomTankPressureData{getOxBottomTankPress0LastSample()}); +} + +void Sensors::oxBottomTankPressure1Callback() +{ + sdLogger.log(OxBottomTankPressureData{getOxBottomTankPress1LastSample()}); +} + +void Sensors::n2TankPressureInit() +{ + n2TankPressure = std::make_unique<TrafagPressureSensor>( + [this]() + { + auto sample = getADS131M08LastSample(); + return sample.getVoltage( + Config::Sensors::ADS131M08::TANK_TOP_PT_CHANNEL); + // TODO: which is the correct channel? + }, + Config::Sensors::Trafag::TANK_TOP_SHUNT_RESISTANCE, + Config::Sensors::Trafag::TANK_TOP_MAX_PRESSURE, + Config::Sensors::Trafag::MIN_CURRENT, + Config::Sensors::Trafag::MAX_CURRENT); + // TODO: is this configuration correct? } -void Sensors::bottomTankPressureCallback() +void Sensors::n2TankPressureCallback() { - sdLogger.log(BottomTankPressureData{getBottomTankPressLastSample()}); + sdLogger.log(N2TankPressureData{getN2TankPressLastSample()}); } void Sensors::ccPressureInit() @@ -438,11 +527,18 @@ bool Sensors::sensorManagerInit() map.emplace(lis2mdl.get(), lis2mdlInfo); } - if (lsm6dsrx) + if (lsm6dsrx0) { - SensorInfo lsm6dsrxInfo{"LSM6DSRX", Config::Sensors::LSM6DSRX::RATE, - [this]() { lsm6dsrxCallback(); }}; - map.emplace(lsm6dsrx.get(), lsm6dsrxInfo); + SensorInfo lsm6dsrxInfo{"LSM6DSRX0", Config::Sensors::LSM6DSRX::RATE, + [this]() { lsm6dsrx0Callback(); }}; + map.emplace(lsm6dsrx0.get(), lsm6dsrxInfo); + } + + if (lsm6dsrx1) + { + SensorInfo lsm6dsrxInfo{"LSM6DSRX1", Config::Sensors::LSM6DSRX::RATE, + [this]() { lsm6dsrx1Callback(); }}; + map.emplace(lsm6dsrx1.get(), lsm6dsrxInfo); } if (ads131m08) @@ -460,18 +556,34 @@ bool Sensors::sensorManagerInit() map.emplace(internalAdc.get(), internalAdcInfo); } - if (topTankPressure) + if (oxTopTankPressure) + { + SensorInfo info{"OxTopTankPressure", Config::Sensors::ADS131M08::RATE, + [this]() { oxTopTankPressureCallback(); }}; + map.emplace(std::make_pair(oxTopTankPressure.get(), info)); + } + + if (oxBottomTankPressure0) + { + SensorInfo info{"OxBottomTankPressure0", + Config::Sensors::ADS131M08::RATE, + [this]() { oxBottomTankPressure0Callback(); }}; + map.emplace(std::make_pair(oxBottomTankPressure0.get(), info)); + } + + if (oxBottomTankPressure1) { - SensorInfo info{"TopTankPressure", Config::Sensors::ADS131M08::RATE, - [this]() { topTankPressureCallback(); }}; - map.emplace(std::make_pair(topTankPressure.get(), info)); + SensorInfo info{"OxBottomTankPressure1", + Config::Sensors::ADS131M08::RATE, + [this]() { oxBottomTankPressure1Callback(); }}; + map.emplace(std::make_pair(oxBottomTankPressure1.get(), info)); } - if (bottomTankPressure) + if (n2TankPressure) { - SensorInfo info{"BottomTankPressure", Config::Sensors::ADS131M08::RATE, - [this]() { bottomTankPressureCallback(); }}; - map.emplace(std::make_pair(bottomTankPressure.get(), info)); + SensorInfo info{"N2TankPressure", Config::Sensors::ADS131M08::RATE, + [this]() { n2TankPressureCallback(); }}; + map.emplace(std::make_pair(n2TankPressure.get(), info)); } if (ccPressure) diff --git a/src/Motor/Sensors/Sensors.h b/src/Motor/Sensors/Sensors.h index ce8b56fd4..810fbfc22 100644 --- a/src/Motor/Sensors/Sensors.h +++ b/src/Motor/Sensors/Sensors.h @@ -54,10 +54,13 @@ public: Boardcore::LPS22DFData getLPS22DFLastSample(); Boardcore::H3LIS331DLData getH3LIS331DLLastSample(); Boardcore::LIS2MDLData getLIS2MDLLastSample(); - Boardcore::LSM6DSRXData getLSM6DSRXLastSample(); + Boardcore::LSM6DSRXData getLSM6DSRX0LastSample(); + Boardcore::LSM6DSRXData getLSM6DSRX1LastSample(); - Boardcore::PressureData getTopTankPressLastSample(); - Boardcore::PressureData getBottomTankPressLastSample(); + Boardcore::PressureData getOxTopTankPressLastSample(); + Boardcore::PressureData getOxBottomTankPress0LastSample(); + Boardcore::PressureData getOxBottomTankPress1LastSample(); + Boardcore::PressureData getN2TankPressLastSample(); Boardcore::PressureData getCCPressLastSample(); Boardcore::TemperatureData getTankTempLastSample(); Boardcore::VoltageData getBatteryVoltageLastSample(); @@ -73,13 +76,16 @@ protected: std::unique_ptr<Boardcore::LPS22DF> lps22df; std::unique_ptr<Boardcore::H3LIS331DL> h3lis331dl; std::unique_ptr<Boardcore::LIS2MDL> lis2mdl; - std::unique_ptr<Boardcore::LSM6DSRX> lsm6dsrx; + std::unique_ptr<Boardcore::LSM6DSRX> lsm6dsrx0; + std::unique_ptr<Boardcore::LSM6DSRX> lsm6dsrx1; std::unique_ptr<Boardcore::ADS131M08> ads131m08; std::unique_ptr<Boardcore::InternalADC> internalAdc; // Analog sensors - std::unique_ptr<Boardcore::TrafagPressureSensor> topTankPressure; - std::unique_ptr<Boardcore::TrafagPressureSensor> bottomTankPressure; + std::unique_ptr<Boardcore::TrafagPressureSensor> oxTopTankPressure; + std::unique_ptr<Boardcore::TrafagPressureSensor> oxBottomTankPressure0; + std::unique_ptr<Boardcore::TrafagPressureSensor> oxBottomTankPressure1; + std::unique_ptr<Boardcore::TrafagPressureSensor> n2TankPressure; std::unique_ptr<Boardcore::TrafagPressureSensor> ccPressure; std::unique_ptr<KuliteThermocouple> tankTemp; @@ -95,8 +101,10 @@ private: void lis2mdlInit(); void lis2mdlCallback(); + /// @brief Initialize both lsm6dsrx sensors. void lsm6dsrxInit(); - void lsm6dsrxCallback(); + void lsm6dsrx0Callback(); + void lsm6dsrx1Callback(); void ads131m08Init(); void ads131m08Callback(); @@ -104,11 +112,16 @@ private: void internalAdcInit(); void internalAdcCallback(); - void topTankPressureInit(); - void topTankPressureCallback(); + void oxTopTankPressureInit(); + void oxTopTankPressureCallback(); - void bottomTankPressureInit(); - void bottomTankPressureCallback(); + /// @brief Initialize both ox bottom pressure sensors. + void oxBottomTankPressureInit(); + void oxBottomTankPressure0Callback(); + void oxBottomTankPressure1Callback(); + + void n2TankPressureInit(); + void n2TankPressureCallback(); void ccPressureInit(); void ccPressureCallback(); diff --git a/src/Motor/Sensors/SensorsData.h b/src/Motor/Sensors/SensorsData.h index b205266fb..168cf941c 100644 --- a/src/Motor/Sensors/SensorsData.h +++ b/src/Motor/Sensors/SensorsData.h @@ -26,24 +26,34 @@ namespace Motor { -struct TopTankPressureData : Boardcore::PressureData +struct OxTopTankPressureData : Boardcore::PressureData { - explicit TopTankPressureData(const Boardcore::PressureData& data) + explicit OxTopTankPressureData(const Boardcore::PressureData& data) : Boardcore::PressureData{data} { } - TopTankPressureData() {} + OxTopTankPressureData() {} }; -struct BottomTankPressureData : Boardcore::PressureData +struct OxBottomTankPressureData : Boardcore::PressureData { - explicit BottomTankPressureData(const Boardcore::PressureData& data) + explicit OxBottomTankPressureData(const Boardcore::PressureData& data) : Boardcore::PressureData{data} { } - BottomTankPressureData() {} + OxBottomTankPressureData() {} +}; + +struct N2TankPressureData : Boardcore::PressureData +{ + explicit N2TankPressureData(const Boardcore::PressureData& data) + : Boardcore::PressureData{data} + { + } + + N2TankPressureData() {} }; struct CCPressureData : Boardcore::PressureData -- GitLab