propagator with acceleration

src/shared/algorithms/Propagator/Propagator.cpp

@@ -31,6 +31,8 @@ using namespace Eigen;
 namespace Boardcore
 {
 
+static constexpr bool useAcceleration = true; //set true to use the propagator with acceleration
+
 Propagator::Propagator(std::chrono::milliseconds pUpdatePeriod)
     : updatePeriod(static_cast<float>(pUpdatePeriod.count()) / 1000), state()
 {
@@ -52,10 +54,24 @@ void Propagator::step()
                                    getRocketNasState())
                  : oldState);
 
-    // Update Position propagating it with velocity
+    if(useAcceleration) // Update Position assuming constant acceleration
+    {
+        // checking that last two states are not propagated
+        if(state.nPropagations == 0 && oldState.nPropagations == 0)             
+            state.setAProp((state.getVProp() - oldState.getVProp()) / updatePeriod);
+
+        state.setVProp((state.getVProp() + state.getAProp()) * updatePeriod);
+        state.setXProp((state.getXProp() + state.getVProp()) * updatePeriod);
+
+        state.nPropagations++;
+        state.timestamp = TimestampTimer::getTimestamp();
+    }
+    else    // Update Position propagating assuming costant velocity
+    { 
     state.setXProp(state.getXProp() + state.getVProp() * updatePeriod);
     state.nPropagations++;
     state.timestamp = TimestampTimer::getTimestamp();
+    }
 
     // Log propagator state
     PropagatorState logState(state);

src/shared/algorithms/Propagator/PropagatorData.h

@@ -42,6 +42,8 @@ struct PropagatorState
     uint32_t nPropagations;  ///< Predictions from last received NAS state
     NASState nas;
 
+    Eigen::Vector3f acceleration = Eigen::Vector3f::Zero(); 
+
     PropagatorState() : timestamp(0), nPropagations(0), nas() {}
 
     PropagatorState(uint64_t timestamp, uint32_t nPropagations,
@@ -102,6 +104,24 @@ struct PropagatorState
         nas.vd = vProp(2);
     }
 
+    /**
+     * @brief Setter for the vector acceleration
+     */
+    void setAProp(Eigen::Vector3f aProp)
+    {
+        acceleration = aProp;
+    }
+
+    /**
+     * @brief Getter for the vector acceleration 
+     *
+     * @return Eigen::Vector3f acceleration
+     */
+    Eigen::Vector3f getAProp() const
+    {
+        return acceleration;
+    }
+
     /**
      * @brief Getter for the vector of quaternions
      *