From b46a9336bbb5aa169bba5cbf7b12d19ccc9a82c1 Mon Sep 17 00:00:00 2001
From: Matteo Pancotti <matteopancotti@Matteos-MacBook-Air.local>
Date: Sun, 9 Feb 2025 02:58:14 +0100
Subject: [PATCH] propagator with acceleration
---
.../algorithms/Propagator/Propagator.cpp | 18 ++++++++++++++++-
.../algorithms/Propagator/PropagatorData.h | 20 +++++++++++++++++++
2 files changed, 37 insertions(+), 1 deletion(-)
diff --git a/src/shared/algorithms/Propagator/Propagator.cpp b/src/shared/algorithms/Propagator/Propagator.cpp
index 8fa10602d..3df6e642a 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 228f306f0..7422e9919 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
*
--
GitLab