From 77c0868e8b6eb27a99275a45c1a52a093f7e4e4c Mon Sep 17 00:00:00 2001
From: Alberto Nidasio <alberto.nidasio@skywarder.eu>
Date: Sun, 11 Sep 2022 13:01:30 +0000
Subject: [PATCH] [Pitot] Implemented reference values

---
 skyward-boardcore                      |  2 +-
 src/boards/Payload/Radio/Radio.cpp     |  2 ++
 src/boards/Payload/Sensors/Sensors.cpp | 22 ++++++++++++++++++++++
 src/boards/Payload/Sensors/Sensors.h   |  3 +++
 4 files changed, 28 insertions(+), 1 deletion(-)

diff --git a/skyward-boardcore b/skyward-boardcore
index 9973d441d..56f049100 160000
--- a/skyward-boardcore
+++ b/skyward-boardcore
@@ -1 +1 @@
-Subproject commit 9973d441dcbf2e88a7ba52b2d35a923da9f5b55a
+Subproject commit 56f0491004203cfee6d417281a82aedb612ee8d7
diff --git a/src/boards/Payload/Radio/Radio.cpp b/src/boards/Payload/Radio/Radio.cpp
index 5f5fe24c4..cba577c5d 100644
--- a/src/boards/Payload/Radio/Radio.cpp
+++ b/src/boards/Payload/Radio/Radio.cpp
@@ -394,6 +394,7 @@ void Radio::handleMavlinkMessage(MavDriver* driver,
                 mavlink_msg_set_reference_altitude_tc_get_ref_altitude(&msg);
 
             NASController::getInstance().setReferenceAltitude(altitude);
+            Sensors::getInstance().pitotSetReferenceAltitude(altitude);
             break;
         }
         case MAVLINK_MSG_ID_SET_REFERENCE_TEMPERATURE_TC:
@@ -402,6 +403,7 @@ void Radio::handleMavlinkMessage(MavDriver* driver,
                 mavlink_msg_set_reference_temperature_tc_get_ref_temp(&msg);
 
             NASController::getInstance().setReferenceTemperature(temperature);
+            Sensors::getInstance().pitotSetReferenceTemperature(temperature);
             break;
         }
         case MAVLINK_MSG_ID_SET_DEPLOYMENT_ALTITUDE_TC:
diff --git a/src/boards/Payload/Sensors/Sensors.cpp b/src/boards/Payload/Sensors/Sensors.cpp
index ec4c9a685..960a28b29 100644
--- a/src/boards/Payload/Sensors/Sensors.cpp
+++ b/src/boards/Payload/Sensors/Sensors.cpp
@@ -169,6 +169,28 @@ void Sensors::calibrate()
     calibrating = true;
 }
 
+void Sensors::pitotSetReferenceAltitude(float altitude)
+{
+    // Need to pause the kernel because the only invocation comes from the radio
+    // which is a separate thread
+    miosix::PauseKernelLock l;
+
+    ReferenceValues reference = pitot->getReferenceValues();
+    reference.refAltitude     = altitude;
+    pitot->setReferenceValues(reference);
+}
+
+void Sensors::pitotSetReferenceTemperature(float temperature)
+{
+    // Need to pause the kernel because the only invocation comes from the radio
+    // which is a separate thread
+    miosix::PauseKernelLock l;
+
+    ReferenceValues reference = pitot->getReferenceValues();
+    reference.refTemperature  = temperature + 273.15f;
+    pitot->setReferenceValues(reference);
+}
+
 std::map<string, bool> Sensors::getSensorsState()
 {
     std::map<string, bool> sensorsState;
diff --git a/src/boards/Payload/Sensors/Sensors.h b/src/boards/Payload/Sensors/Sensors.h
index 0a478e192..97b839700 100644
--- a/src/boards/Payload/Sensors/Sensors.h
+++ b/src/boards/Payload/Sensors/Sensors.h
@@ -73,6 +73,9 @@ public:
      */
     void calibrate();
 
+    void pitotSetReferenceAltitude(float altitude);
+    void pitotSetReferenceTemperature(float temperature);
+
     std::map<string, bool> getSensorsState();
 
 private:
-- 
GitLab