[ARP]PropagatorData could not be logged, propagator algorithm improved

src/shared/algorithms/Propagator/Propagator.cpp

@@ -46,6 +46,7 @@ void Propagator::step()
     miosix::Lock<miosix::FastMutex> lock(stateMutex);
     // Take new rocket data only if it has been just updated, otherwise take
     // last state available
+
     PropagatorState oldState = state;
 
     // updates with the last received NAS state if present, otherwise uses the
@@ -55,14 +56,22 @@ void Propagator::step()
                                    getRocketNasState())
                  : oldState);
 
-    if (useAcceleration)  // Update Position assuming constant acceleration
+    if (state.nPropagations == 0 &&
+        useAcceleration)  // Update Position assuming constant acceleration
     {
-        state.setAcceleration((state.getVelocity() - oldState.getVelocity()) /
-                              updatePeriod);
-        state.setVelocity((state.getVelocity() + state.getAcceleration()) *
-                          updatePeriod);
+        t1 = state.timestamp;
+        dt = t1 - t0;
+        if (dt > 0 && dt < 4646476 && t0 != 0)
+            state.setAcceleration(state.getVelocity() -
+                                  last_real_velocity / dt);
+        t0                 = t1;
+        last_real_velocity = state.getVelocity();
     }
 
+    if (useAcceleration)
+        state.setVelocity(state.getVelocity() +
+                          state.getAcceleration() * updatePeriod);
+
     state.setPosition(state.getPosition() + state.getVelocity() * updatePeriod);
 
     state.nPropagations++;

src/shared/algorithms/Propagator/Propagator.h

@@ -97,6 +97,8 @@ 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;
 };
 
 }  // namespace Boardcore

src/shared/algorithms/Propagator/PropagatorData.h

@@ -42,7 +42,10 @@ struct PropagatorState
     uint32_t nPropagations;  ///< Predictions from last received NAS state
     NASState nas;
 
-    Eigen::Vector3f acceleration = Eigen::Vector3f::Zero();
+    
+    float ax = 0, ay = 0,
+          az = 0;  // propagater acceleration (Eigen::Vector3f could not be used
+                   // because it is not trivially copyable)
 
     PropagatorState() : timestamp(0), nPropagations(0), nas() {}
 
@@ -63,9 +66,8 @@ struct PropagatorState
         os << timestamp << "," << nPropagations << "," << nas.n << "," << nas.e
            << "," << nas.d << "," << nas.vn << "," << nas.ve << "," << nas.vd
            << "," << nas.qx << "," << nas.qy << "," << nas.qz << "," << nas.qw
-           << "," << nas.bx << "," << nas.by << "," << nas.bz << ","
-           << acceleration[0] << "," << acceleration[1] << ","
-           << acceleration[2] << "\n";
+           << "," << nas.bx << "," << nas.by << "," << nas.bz << "," << ax
+           << "," << ay << "," << az << "\n";
     }
 
     NASState getNasState() const { return nas; }
@@ -113,14 +115,26 @@ struct PropagatorState
     /**
      * @brief Setter for the vector acceleration
      */
-    void setAcceleration(Eigen::Vector3f acc) { acceleration = acc; }
+    void setAcceleration(Eigen::Vector3f acc)
+    {
+        ax = acc[0];
+        ay = acc[1];
+        az = acc[2];
+    }
 
     /**
      * @brief Getter for the vector acceleration
      *
      * @return Eigen::Vector3f acceleration
      */
-    Eigen::Vector3f getAcceleration() const { return acceleration; }
+    Eigen::Vector3f getAcceleration() const
+    {
+        Eigen::Vector3f acc;
+        acc[0] = ax;
+        acc[1] = ay;
+        acc[2] = az;
+        return acc;
+    }
 
     /**
      * @brief Getter for the vector of quaternions