From e2d8cc81345533753b81c1c8ed0e61fc3ba4b0a2 Mon Sep 17 00:00:00 2001
From: Matteo Pancotti <matteopancotti@Matteos-MacBook-Air.local>
Date: Sun, 4 May 2025 16:28:55 +0200
Subject: [PATCH] Last changes before merge

Now only z-axis acceleration is used to propagate + some comments added.
---
 .../algorithms/Propagator/Propagator.cpp      |  6 ++---
 src/shared/algorithms/Propagator/Propagator.h |  8 +++++--
 .../algorithms/Propagator/PropagatorData.h    | 22 +++++++------------
 3 files changed, 17 insertions(+), 19 deletions(-)

diff --git a/src/shared/algorithms/Propagator/Propagator.cpp b/src/shared/algorithms/Propagator/Propagator.cpp
index fa537373f..c9fe1f67a 100644
--- a/src/shared/algorithms/Propagator/Propagator.cpp
+++ b/src/shared/algorithms/Propagator/Propagator.cpp
@@ -61,9 +61,9 @@ void Propagator::step()
     {
         t1 = state.timestamp;
         dt = t1 - t0;
-        if (dt > 0 && dt < 4646476 && t0 != 0)
-            state.setAcceleration(state.getVelocity() -
-                                  last_real_velocity / dt);
+        if (dt > 0 && dt < maxAccelerationTime && t0 != 0)
+            state.setZAcceleration((state.getVelocity() - last_real_velocity) /
+                                   dt);
         t0                 = t1;
         last_real_velocity = state.getVelocity();
     }
diff --git a/src/shared/algorithms/Propagator/Propagator.h b/src/shared/algorithms/Propagator/Propagator.h
index 2f2b4860b..24968096c 100644
--- a/src/shared/algorithms/Propagator/Propagator.h
+++ b/src/shared/algorithms/Propagator/Propagator.h
@@ -97,8 +97,12 @@ private:
     NASState lastRocketNasState;      ///< Last received rocket NAS state
     miosix::FastMutex nasStateMutex;  ///< mutex to sync nasState accesses
     miosix::FastMutex stateMutex;     ///< mutex to sync state accesses
-    Eigen::Vector3f last_real_velocity;
-    uint64_t t0 = 0, t1 = 0, dt = 0;
+    Eigen::Vector3f last_real_velocity;  ///< last non-propagated velocity
+    uint64_t t0 = 0, t1 = 0,
+             dt = 0;  ///< time values used to compute acceleration [u]
+    const uint64_t maxAccelerationTime =
+        5000000;  ///< Time limit after which acceleration data is ignored for
+                  ///< propagation (5s)
 };
 
 }  // namespace Boardcore
diff --git a/src/shared/algorithms/Propagator/PropagatorData.h b/src/shared/algorithms/Propagator/PropagatorData.h
index 542d62e01..cc1ff643f 100644
--- a/src/shared/algorithms/Propagator/PropagatorData.h
+++ b/src/shared/algorithms/Propagator/PropagatorData.h
@@ -42,10 +42,9 @@ struct PropagatorState
     uint32_t nPropagations;  ///< Predictions from last received NAS state
     NASState nas;
 
-    float ax = 0, ay = 0,
-          az = 0;  // propagater acceleration (Eigen::Vector3f could not be used
-                   // because it is not trivially copyable)
-
+    float az                  = 0;
+    static constexpr float ax = 0,
+                           ay = 0;  ///< only az is used by the propagator
     PropagatorState() : timestamp(0), nPropagations(0), nas() {}
 
     PropagatorState(uint64_t timestamp, uint32_t nPropagations,
@@ -112,14 +111,9 @@ struct PropagatorState
     }
 
     /**
-     * @brief Setter for the vector acceleration
+     * @brief Setter for the vector acceleration(only z-axis)
      */
-    void setAcceleration(Eigen::Vector3f acc)
-    {
-        ax = acc[0];
-        ay = acc[1];
-        az = acc[2];
-    }
+    void setZAcceleration(Eigen::Vector3f acc) { az = acc(2); }
 
     /**
      * @brief Getter for the vector acceleration
@@ -129,9 +123,9 @@ struct PropagatorState
     Eigen::Vector3f getAcceleration() const
     {
         Eigen::Vector3f acc;
-        acc[0] = ax;
-        acc[1] = ay;
-        acc[2] = az;
+        acc(0) = ax;
+        acc(1) = ay;
+        acc(2) = az;
         return acc;
     }
 
-- 
GitLab