From c9a7ecc12c05c50de481ed48e278b850bba06de6 Mon Sep 17 00:00:00 2001
From: Alberto Nidasio <alberto.nidasio@skywarder.eu>
Date: Mon, 26 Sep 2022 22:14:12 +0200
Subject: [PATCH] [ADA] Fixed initial altitude

---
 .../Main/StateMachines/ADAController/ADAController.cpp      | 6 +++++-
 .../Main/StateMachines/NASController/NASController.cpp      | 6 +++++-
 2 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.cpp b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
index b72730bf3..dc31b481b 100644
--- a/src/boards/Main/StateMachines/ADAController/ADAController.cpp
+++ b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
@@ -33,6 +33,7 @@
 #include <drivers/timer/TimestampTimer.h>
 #include <events/EventBroker.h>
 #include <miosix.h>
+#include <utils/AeroUtils/AeroUtils.h>
 
 using namespace miosix;
 using namespace Boardcore;
@@ -223,7 +224,7 @@ void ADAController::calibrate()
 
     for (int i = 0; i < CALIBRATION_SAMPLES_COUNT; i++)
     {
-        auto data = Sensors::getInstance().getMS5803LastSample();
+        auto data = Sensors::getInstance().getStaticPressureLastSample();
         pressure.add(data.pressure);
 
         miosix::Thread::sleep(CALIBRATION_SLEEP_TIME);
@@ -232,12 +233,15 @@ void ADAController::calibrate()
     // Set the pressure and temperature reference
     ReferenceValues reference = ada.getReferenceValues();
     reference.refPressure     = pressure.getStats().mean;
+    reference.refAltitude     = Aeroutils::relAltitude(
+            reference.refPressure, reference.mslPressure, reference.mslTemperature);
 
     // Update the algorithm reference values
     {
         miosix::PauseKernelLock l;
         ada.setReferenceValues(reference);
         ada.setKalmanConfig(getADAKalmanConfig());
+        ada.update(reference.refPressure);
     }
 
     EventBroker::getInstance().post(ADA_READY, TOPIC_ADA);
diff --git a/src/boards/Main/StateMachines/NASController/NASController.cpp b/src/boards/Main/StateMachines/NASController/NASController.cpp
index 2cc7b63b6..cf39b26e0 100644
--- a/src/boards/Main/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Main/StateMachines/NASController/NASController.cpp
@@ -30,6 +30,7 @@
 #include <algorithms/NAS/StateInitializer.h>
 #include <common/ReferenceConfig.h>
 #include <common/events/Events.h>
+#include <utils/AeroUtils/AeroUtils.h>
 
 using namespace std;
 using namespace Eigen;
@@ -104,7 +105,8 @@ void NASController::calibrate()
                      imuData.magneticFieldZ);
 
         // Barometer
-        MS5803Data barometerData = Sensors::getInstance().getMS5803LastSample();
+        auto barometerData =
+            Sensors::getInstance().getStaticPressureLastSample();
         pressure.add(barometerData.pressure);
 
         miosix::Thread::sleep(CALIBRATION_SLEEP_TIME);
@@ -123,6 +125,8 @@ void NASController::calibrate()
     // Set the pressure reference using an already existing reference values
     ReferenceValues reference = nas.getReferenceValues();
     reference.refPressure     = pressure.getStats().mean;
+    reference.refAltitude     = Aeroutils::relAltitude(
+            reference.refPressure, reference.mslPressure, reference.mslTemperature);
 
     // If in this moment the GPS has fix i use that position as starting
     UBXGPSData gps = Sensors::getInstance().getUbxGpsLastSample();
-- 
GitLab