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