diff --git a/src/shared/algorithms/Propagator/Propagator.cpp b/src/shared/algorithms/Propagator/Propagator.cpp index 8fa10602dffb0ab12f8e11f5485cd8a857b98f91..3df6e642a9a7b1e0e6db1c637c6a5923e327c44c 100644 --- a/src/shared/algorithms/Propagator/Propagator.cpp +++ b/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); diff --git a/src/shared/algorithms/Propagator/PropagatorData.h b/src/shared/algorithms/Propagator/PropagatorData.h index 228f306f047a586811061fda71a2d51dbe16be1d..7422e99196e8de69f9f1316b6b09d00b4a540fe5 100644 --- a/src/shared/algorithms/Propagator/PropagatorData.h +++ b/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 *