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