From 2a06a5e1118cd24377597b8f0a68c9996904318a Mon Sep 17 00:00:00 2001
From: Alberto Nidasio <alberto.nidasio@skywarder.eu>
Date: Sun, 11 Sep 2022 07:09:41 +0000
Subject: [PATCH] [NAS] Pitot correction now applied only during ascent

---
 .../Main/StateMachines/NASController/NASController.cpp     | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/boards/Main/StateMachines/NASController/NASController.cpp b/src/boards/Main/StateMachines/NASController/NASController.cpp
index 8704d2bc7..bd5d9bcf8 100644
--- a/src/boards/Main/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Main/StateMachines/NASController/NASController.cpp
@@ -25,6 +25,7 @@
 #include <Main/BoardScheduler.h>
 #include <Main/Configs/NASConfig.h>
 #include <Main/Sensors/Sensors.h>
+#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
 #include <algorithms/NAS/StateInitializer.h>
 #include <common/ReferenceConfig.h>
 #include <common/events/Events.h>
@@ -68,7 +69,11 @@ void NASController::update()
         nas.correctMag(imuData);
         nas.correctGPS(gpsData);
         nas.correctBaro(pressureData.pressure);
-        nas.correctPitot(pitotData.deltaP, pressureData.pressure);
+
+        // Correct the pitot only during ascending
+        if (FlightModeManager::getInstance().getStatus().state ==
+            FlightModeManagerState::ASCENDING)
+            nas.correctPitot(pitotData.deltaP, pressureData.pressure);
 
         Logger::getInstance().log(nas.getState());
     }
-- 
GitLab