[Payload][NAS] Update with Pitot correction

src/boards/Payload/Configs/NASConfig.h

@@ -47,11 +47,11 @@ static const Boardcore::NASConfig CONFIG = {
     .SIGMA_W        = 0.3,
     .SIGMA_ACC      = 0.1,
     .SIGMA_MAG      = 0.1,
-    .SIGMA_GPS      = {0.0447f, 0.0447f, 0.1f / 30.0f, 0.1f / 30.0f},
-    .SIGMA_BAR      = 4.3,
+    .SIGMA_GPS      = {0.002f, 0.002f, 0.01f / 30.0f, 0.01f / 30.0f},
+    .SIGMA_BAR      = 50.0,
     .SIGMA_POS      = 10.0,
     .SIGMA_VEL      = 10.0,
-    .SIGMA_PITOT    = 10.0,
+    .SIGMA_PITOT    = 10e-3,
     .P_POS          = 1.0,
     .P_POS_VERTICAL = 10.0,
     .P_VEL          = 1.0,
@@ -74,6 +74,12 @@ constexpr float ACCELERATION_1G_CONFIDENCE = 0.5;
 // acceleration
 constexpr int ACCELERATION_1G_SAMPLES = 20;
 
+// Altitude under which we make the pitot correction
+constexpr float PITOT_ALTITUDE_THRESHOLD = 800;  // [m]
+
+// Vertical speed over which we make the pitot correction
+constexpr float PITOT_SPEED_THRESHOLD = 70;  // [m/s]
+
 }  // namespace NAS
 }  // namespace Config
 }  // namespace Payload

src/boards/Payload/StateMachines/NASController/NASController.cpp

@@ -44,33 +44,29 @@ NASController::NASController()
           BoardScheduler::nasControllerPriority()),
       nas(config::CONFIG)
 {
-    // Subscribe the class to the topics
     EventBroker::getInstance().subscribe(this, TOPIC_NAS);
     EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
+}
 
+bool NASController::start()
+{
     // Setup the NAS
     Matrix<float, 13, 1> x = Matrix<float, 13, 1>::Zero();
-
     // Create the initial quaternion
     Vector4f q = SkyQuaternion::eul2quat({0, 0, 0});
 
     // Set the initial quaternion inside the matrix
-    x(6) = q(0);
-    x(7) = q(1);
-    x(8) = q(2);
-    x(9) = q(3);
+    x(NAS::IDX_QUAT + 0) = q(0);
+    x(NAS::IDX_QUAT + 1) = q(1);
+    x(NAS::IDX_QUAT + 2) = q(2);
+    x(NAS::IDX_QUAT + 3) = q(3);
 
     // Set the NAS x matrix
     nas.setX(x);
-
-    // Set the referenced values for the correct place on earth
+    // Set the initial reference values from the default ones
     nas.setReferenceValues(ReferenceConfig::defaultReferenceValues);
-}
 
-bool NASController::start()
-{
     auto& scheduler = getModule<BoardScheduler>()->nasController();
-
     // Add the task to the scheduler
     auto task = scheduler.addTask([this] { update(); }, config::UPDATE_RATE,
                                   TaskScheduler::Policy::RECOVER);
@@ -265,13 +261,18 @@ void NASController::calibrate()
 void NASController::update()
 {
     // Update the NAS state only if the FSM is active
-    if (state == NASControllerState::ACTIVE)
+    if (state != NASControllerState::ACTIVE)
     {
-        Sensors* sensors = getModule<Sensors>();
+        return;
+    }
 
-        IMUData imu       = sensors->getIMULastSample();
-        UBXGPSData gps    = sensors->getUBXGPSLastSample();
-        PressureData baro = sensors->getLPS28DFWLastSample();
+    auto* sensors = getModule<Sensors>();
+
+    auto imu          = sensors->getIMULastSample();
+    auto gps          = sensors->getUBXGPSLastSample();
+    auto baro         = sensors->getLPS28DFWLastSample();
+    auto staticPitot  = sensors->getStaticPressureLastSample();
+    auto dynamicPitot = sensors->getDynamicPressureLastSample();
 
     // Calculate acceleration
     Vector3f acc    = static_cast<AccelerometerData>(imu);
@@ -280,7 +281,6 @@ void NASController::update()
     miosix::Lock<miosix::FastMutex> l(nasMutex);
 
     // Perform initial NAS prediction
-        // TODO: What about stale data?
     nas.predictGyro(imu);
     nas.predictAcc(imu);
 
@@ -307,7 +307,15 @@ void NASController::update()
         nas.correctBaro(baro.pressure);
     }
 
-        // TODO: Correct with pitot
+    // Correct with pitot if one pressure sample is new
+    if (dynamicPitot.pressure > 0 &&
+        (staticPitotTimestamp < staticPitot.pressureTimestamp ||
+         dynamicPitotTimestamp < dynamicPitot.pressureTimestamp) &&
+        (-nas.getState().d < Config::NAS::PITOT_ALTITUDE_THRESHOLD) &&
+        (-nas.getState().vd > Config::NAS::PITOT_SPEED_THRESHOLD))
+    {
+        nas.correctPitot(staticPitot.pressure, dynamicPitot.pressure);
+    }
 
     // Correct with accelerometer if the acceleration is in specs
     if (lastAccTimestamp < imu.accelerationTimestamp && acc1g)
@@ -341,13 +349,14 @@ void NASController::update()
     lastMagTimestamp      = imu.magneticFieldTimestamp;
     lastGpsTimestamp      = gps.gpsTimestamp;
     lastBaroTimestamp     = baro.pressureTimestamp;
+    staticPitotTimestamp  = staticPitot.pressureTimestamp;
+    dynamicPitotTimestamp = dynamicPitot.pressureTimestamp;
 
     auto state = nas.getState();
 
     getModule<FlightStatsRecorder>()->updateNas(state);
     Logger::getInstance().log(state);
 }
-}
 
 void NASController::updateState(NASControllerState newState)
 {

src/boards/Payload/StateMachines/NASController/NASController.h

@@ -91,6 +91,8 @@ private:
     uint64_t lastMagTimestamp      = 0;
     uint64_t lastGpsTimestamp      = 0;
     uint64_t lastBaroTimestamp     = 0;
+    uint64_t staticPitotTimestamp  = 0;
+    uint64_t dynamicPitotTimestamp = 0;
 
     std::atomic<bool> started{false};