From 52e88602acaa82af8dfc8b31b0a9a9f666594d6e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Niccol=C3=B2=20Betto?= <niccolo.betto@skywarder.eu>
Date: Tue, 8 Apr 2025 00:17:19 +0200
Subject: [PATCH] [RIGv2] Cleanup OX tank pressures

Apparently there still are top and bottom pressures for the OX tank...
---
 src/RIGv2/CanHandler/CanHandler.cpp     |  4 +-
 src/RIGv2/Radio/Radio.cpp               | 23 ++++++------
 src/RIGv2/Sensors/Sensors.cpp           | 50 +++++++++++++++----------
 src/RIGv2/Sensors/Sensors.h             | 17 +++++----
 src/RIGv2/StateMachines/TARS1/TARS1.cpp |  2 +-
 5 files changed, 56 insertions(+), 40 deletions(-)

diff --git a/src/RIGv2/CanHandler/CanHandler.cpp b/src/RIGv2/CanHandler/CanHandler.cpp
index ddfefe9bd..42494b83b 100644
--- a/src/RIGv2/CanHandler/CanHandler.cpp
+++ b/src/RIGv2/CanHandler/CanHandler.cpp
@@ -213,7 +213,7 @@ void CanHandler::handleSensor(const Canbus::CanMessage& msg)
         {
             CanPressureData data = pressureDataFromCanMessage(msg);
             sdLogger.log(data);
-            sensors->setCanOxTankPressure(data);
+            sensors->setCanOxTankTopPressure(data);
             break;
         }
 
@@ -221,7 +221,7 @@ void CanHandler::handleSensor(const Canbus::CanMessage& msg)
         {
             CanPressureData data = pressureDataFromCanMessage(msg);
             sdLogger.log(data);
-            sensors->setCanN2TankPressure(data);
+            sensors->setCanOxTankBottomPressure(data);
             break;
         }
 
diff --git a/src/RIGv2/Radio/Radio.cpp b/src/RIGv2/Radio/Radio.cpp
index 944fab614..26bce81fe 100644
--- a/src/RIGv2/Radio/Radio.cpp
+++ b/src/RIGv2/Radio/Radio.cpp
@@ -654,10 +654,11 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
             tm.timestamp = TimestampTimer::getTimestamp();
 
             // Sensors (either CAN or local)
-            tm.ox_tank_top_pressure = sensors->getN2TankPressure()
-                                          .pressure;  // TODO: rename in mavlink
-            tm.ox_tank_bot_pressure = sensors->getOxTankPressure()
-                                          .pressure;  // TODO: rename in mavlink
+            tm.n2_tank_pressure = sensors->getN2TankPressure().pressure;
+            tm.ox_tank_bot_pressure =
+                sensors->getOxTankBottomPressure().pressure;
+            tm.ox_tank_top_pressure =
+                sensors->getCanOxTankTopPressure().pressure;
             tm.combustion_chamber_pressure =
                 sensors->getCombustionChamberPressure().pressure;
             tm.ox_tank_temperature =
@@ -765,7 +766,7 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
 
             tm.timestamp = data.pressureTimestamp;
             tm.pressure  = data.pressure;
-            strcpy(tm.sensor_name, "VesselPressure");
+            strcpy(tm.sensor_name, "OxVesselPressure");
 
             mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID,
                                            Config::Radio::MAV_COMPONENT_ID,
@@ -783,7 +784,7 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
 
             tm.timestamp = data.pressureTimestamp;
             tm.pressure  = data.pressure;
-            strcpy(tm.sensor_name, "FillingPressure");
+            strcpy(tm.sensor_name, "OxFillingPressure");
 
             mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID,
                                            Config::Radio::MAV_COMPONENT_ID,
@@ -797,11 +798,11 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
             mavlink_message_t msg;
             mavlink_pressure_tm_t tm;
 
-            PressureData data = getModule<Sensors>()->getOxTankPressure();
+            PressureData data = getModule<Sensors>()->getOxTankBottomPressure();
 
             tm.timestamp = data.pressureTimestamp;
             tm.pressure  = data.pressure;
-            strcpy(tm.sensor_name, "BottomTankPressure");
+            strcpy(tm.sensor_name, "OxTankBotPressure");
 
             mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID,
                                            Config::Radio::MAV_COMPONENT_ID,
@@ -815,11 +816,11 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
             mavlink_message_t msg;
             mavlink_pressure_tm_t tm;
 
-            PressureData data = getModule<Sensors>()->getN2TankPressure();
+            PressureData data = getModule<Sensors>()->getCanOxTankTopPressure();
 
             tm.timestamp = data.pressureTimestamp;
             tm.pressure  = data.pressure;
-            strcpy(tm.sensor_name, "TopTankPressure");
+            strcpy(tm.sensor_name, "OxTankTopPressure");
 
             mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID,
                                            Config::Radio::MAV_COMPONENT_ID,
@@ -855,7 +856,7 @@ bool Radio::enqueueSensorTm(uint8_t tmId)
 
             tm.timestamp = data.loadTimestamp;
             tm.load      = data.load;
-            strcpy(tm.sensor_name, "VesselWeight");
+            strcpy(tm.sensor_name, "OxVesselWeight");
 
             mavlink_msg_load_tm_encode(Config::Radio::MAV_SYSTEM_ID,
                                        Config::Radio::MAV_COMPONENT_ID, &msg,
diff --git a/src/RIGv2/Sensors/Sensors.cpp b/src/RIGv2/Sensors/Sensors.cpp
index 614a961aa..89e22b649 100644
--- a/src/RIGv2/Sensors/Sensors.cpp
+++ b/src/RIGv2/Sensors/Sensors.cpp
@@ -56,7 +56,7 @@ bool Sensors::start()
     if (Config::Sensors::ADC_2::ENABLED)
     {
         adc2Init();
-        oxTankPressureInit();
+        oxTankBottomPressureInit();
         n2TankPressureInit();
     }
 
@@ -123,16 +123,16 @@ PressureData Sensors::getN2FillingPressure()
                              : PressureData{};
 }
 
-PressureData Sensors::getOxTankPressure()
+PressureData Sensors::getOxTankBottomPressure()
 {
     if (useCanData)
     {
-        return getCanOxTankPressure();
+        return getCanOxTankBottomPressure();
     }
     else
     {
-        return oxTankPressure ? oxTankPressure->getLastSample()
-                              : PressureData{};
+        return oxTankBottomPressure ? oxTankBottomPressure->getLastSample()
+                                    : PressureData{};
     }
 }
 
@@ -223,10 +223,16 @@ PressureData Sensors::getCanN2TankPressure()
     return canN2TankPressure;
 }
 
-PressureData Sensors::getCanOxTankPressure()
+PressureData Sensors::getCanOxTankBottomPressure()
+{
+    Lock<FastMutex> lock{canMutex};
+    return canOxTankBottomPressure;
+}
+
+PressureData Sensors::getCanOxTankTopPressure()
 {
     Lock<FastMutex> lock{canMutex};
-    return canOxTankPressure;
+    return canOxTankTopPressure;
 }
 
 PressureData Sensors::getCanCombustionChamberPressure()
@@ -247,16 +253,22 @@ VoltageData Sensors::getCanMotorBatteryVoltage()
     return canMotorBatteryVoltage;
 }
 
-void Sensors::setCanOxTankPressure(PressureData data)
+void Sensors::setCanOxTankBottomPressure(PressureData data)
 {
     Lock<FastMutex> lock{canMutex};
-    canN2TankPressure = data;
+    canOxTankBottomPressure = data;
+}
+
+void Sensors::setCanOxTankTopPressure(PressureData data)
+{
+    Lock<FastMutex> lock{canMutex};
+    canOxTankTopPressure = data;
 }
 
 void Sensors::setCanN2TankPressure(PressureData data)
 {
     Lock<FastMutex> lock{canMutex};
-    canOxTankPressure = data;
+    canN2TankPressure = data;
 }
 
 void Sensors::setCanCombustionChamberPressure(PressureData data)
@@ -320,7 +332,7 @@ std::vector<SensorInfo> Sensors::getSensorInfos()
         PUSH_SENSOR_INFO(n2Vessel1Pressure, "N2Vessel1Pressure");
         PUSH_SENSOR_INFO(n2Vessel2Pressure, "N2Vessel2Pressure");
         PUSH_SENSOR_INFO(n2FillingPressure, "N2FillingPressure");
-        PUSH_SENSOR_INFO(oxTankPressure, "OxTankPressure");
+        PUSH_SENSOR_INFO(oxTankBottomPressure, "OxTankBotPressure");
         PUSH_SENSOR_INFO(n2TankPressure, "N2TankPressure");
         PUSH_SENSOR_INFO(oxVesselWeight, "OxVesselWeight");
         PUSH_SENSOR_INFO(rocketWeight, "RocketWeight");
@@ -571,9 +583,9 @@ void Sensors::n2FillingPressureCallback()
     sdLogger.log(N2FillingPressureData{getN2FillingPressure()});
 }
 
-void Sensors::oxTankPressureInit()
+void Sensors::oxTankBottomPressureInit()
 {
-    oxTankPressure = std::make_unique<TrafagPressureSensor>(
+    oxTankBottomPressure = std::make_unique<TrafagPressureSensor>(
         [this]()
         {
             auto sample = getADC2LastSample();
@@ -586,9 +598,9 @@ void Sensors::oxTankPressureInit()
         Config::Sensors::Trafag::MAX_CURRENT);
 }
 
-void Sensors::oxTankPressureCallback()
+void Sensors::oxTankBottomPressureCallback()
 {
-    sdLogger.log(OxTankPressureData{oxTankPressure->getLastSample()});
+    sdLogger.log(OxTankPressureData{oxTankBottomPressure->getLastSample()});
 }
 
 void Sensors::n2TankPressureInit()
@@ -739,11 +751,11 @@ bool Sensors::sensorManagerInit()
         map.emplace(std::make_pair(n2FillingPressure.get(), info));
     }
 
-    if (oxTankPressure)
+    if (oxTankBottomPressure)
     {
-        SensorInfo info("OxTankPressure", Config::Sensors::ADS131M08::PERIOD,
-                        [this]() { oxTankPressureCallback(); });
-        map.emplace(std::make_pair(oxTankPressure.get(), info));
+        SensorInfo info("OxTankBotPressure", Config::Sensors::ADS131M08::PERIOD,
+                        [this]() { oxTankBottomPressureCallback(); });
+        map.emplace(std::make_pair(oxTankBottomPressure.get(), info));
     }
 
     if (n2TankPressure)
diff --git a/src/RIGv2/Sensors/Sensors.h b/src/RIGv2/Sensors/Sensors.h
index f7f9bffe5..1c3a2dba8 100644
--- a/src/RIGv2/Sensors/Sensors.h
+++ b/src/RIGv2/Sensors/Sensors.h
@@ -64,7 +64,7 @@ public:
     Boardcore::PressureData getN2Vessel1Pressure();
     Boardcore::PressureData getN2Vessel2Pressure();
     Boardcore::PressureData getN2FillingPressure();
-    Boardcore::PressureData getOxTankPressure();
+    Boardcore::PressureData getOxTankBottomPressure();
     Boardcore::PressureData getN2TankPressure();
     Boardcore::PressureData getCombustionChamberPressure();
 
@@ -78,7 +78,8 @@ public:
     Boardcore::VoltageData getBatteryVoltage();
     Boardcore::VoltageData getMotorBatteryVoltage();
 
-    Boardcore::PressureData getCanOxTankPressure();
+    Boardcore::PressureData getCanOxTankBottomPressure();
+    Boardcore::PressureData getCanOxTankTopPressure();
     Boardcore::PressureData getCanN2TankPressure();
     Boardcore::PressureData getCanCombustionChamberPressure();
     Boardcore::TemperatureData getCanTankTemperature();
@@ -86,7 +87,8 @@ public:
 
     std::vector<Boardcore::SensorInfo> getSensorInfos();
 
-    void setCanOxTankPressure(Boardcore::PressureData data);
+    void setCanOxTankBottomPressure(Boardcore::PressureData data);
+    void setCanOxTankTopPressure(Boardcore::PressureData data);
     void setCanN2TankPressure(Boardcore::PressureData data);
     void setCanCombustionChamberPressure(Boardcore::PressureData data);
     void setCanOxTankTemperature(Boardcore::TemperatureData data);
@@ -109,8 +111,8 @@ private:
     void n2FillingPressureInit();
     void n2FillingPressureCallback();
 
-    void oxTankPressureInit();
-    void oxTankPressureCallback();
+    void oxTankBottomPressureInit();
+    void oxTankBottomPressureCallback();
 
     void n2TankPressureInit();
     void n2TankPressureCallback();
@@ -146,7 +148,8 @@ private:
     std::atomic<bool> useCanData{false};
     miosix::FastMutex canMutex;
 
-    Boardcore::PressureData canOxTankPressure;
+    Boardcore::PressureData canOxTankBottomPressure;
+    Boardcore::PressureData canOxTankTopPressure;
     Boardcore::PressureData canN2TankPressure;
     Boardcore::PressureData canCombustionChamberPressure;
     // TODO: N2 tank pressure from CAN
@@ -159,7 +162,7 @@ private:
     std::unique_ptr<Boardcore::TrafagPressureSensor> n2Vessel1Pressure;
     std::unique_ptr<Boardcore::TrafagPressureSensor> n2Vessel2Pressure;
     std::unique_ptr<Boardcore::TrafagPressureSensor> n2FillingPressure;
-    std::unique_ptr<Boardcore::TrafagPressureSensor> oxTankPressure;
+    std::unique_ptr<Boardcore::TrafagPressureSensor> oxTankBottomPressure;
     std::unique_ptr<Boardcore::TrafagPressureSensor> n2TankPressure;
     std::unique_ptr<Boardcore::TwoPointAnalogLoadCell> oxVesselWeight;
     std::unique_ptr<Boardcore::TwoPointAnalogLoadCell> rocketWeight;
diff --git a/src/RIGv2/StateMachines/TARS1/TARS1.cpp b/src/RIGv2/StateMachines/TARS1/TARS1.cpp
index bc092337b..2d1e27c00 100644
--- a/src/RIGv2/StateMachines/TARS1/TARS1.cpp
+++ b/src/RIGv2/StateMachines/TARS1/TARS1.cpp
@@ -257,7 +257,7 @@ void TARS1::sample()
 {
     Sensors* sensors = getModule<Sensors>();
 
-    pressureFilter.add(sensors->getOxTankPressure().pressure);
+    pressureFilter.add(sensors->getOxTankBottomPressure().pressure);
     massFilter.add(sensors->getOxTankWeight().load);
     medianSamples++;
 
-- 
GitLab