From cc81cf44e33ca625ed0395066ede6138294ae855 Mon Sep 17 00:00:00 2001
From: Matteo Pancotti <matteopancotti@Matteos-MacBook-Air.local>
Date: Sun, 16 Feb 2025 16:57:12 +0100
Subject: [PATCH] [ARP] suggested changes
---
.../algorithms/Propagator/Propagator.cpp | 23 +++++++---------
.../algorithms/Propagator/PropagatorData.h | 26 +++++++++----------
2 files changed, 22 insertions(+), 27 deletions(-)
diff --git a/src/shared/algorithms/Propagator/Propagator.cpp b/src/shared/algorithms/Propagator/Propagator.cpp
index 3c4c50439..be54a81fc 100644
--- a/src/shared/algorithms/Propagator/Propagator.cpp
+++ b/src/shared/algorithms/Propagator/Propagator.cpp
@@ -31,7 +31,8 @@ using namespace Eigen;
namespace Boardcore
{
-static constexpr bool useAcceleration = true; //set true to use the propagator with acceleration
+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()
@@ -54,24 +55,18 @@ void Propagator::step()
getRocketNasState())
: oldState);
- if(useAcceleration) // Update Position assuming constant acceleration
+ if (useAcceleration) // Update Position assuming constant acceleration
{
- // checking that last two states are not propagated
- if(state.nPropagations == 0 && oldState.nPropagations == 0)
- state.setAcceleration((state.getVelocity() - oldState.getVelocity()) / updatePeriod);
-
- state.setVelocity((state.getVelocity() + state.getAcceleration()) * updatePeriod);
- state.setPosition((state.getPosition() + state.getVelocity()) * updatePeriod);
-
- state.nPropagations++;
- state.timestamp = TimestampTimer::getTimestamp();
+ state.setAcceleration((state.getVelocity() - oldState.getVelocity()) /
+ updatePeriod);
+ state.setVelocity((state.getVelocity() + state.getAcceleration()) *
+ updatePeriod);
}
- else // Update Position propagating assuming costant velocity
- {
+
state.setPosition(state.getPosition() + state.getVelocity() * 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 63389f511..8df9949d6 100644
--- a/src/shared/algorithms/Propagator/PropagatorData.h
+++ b/src/shared/algorithms/Propagator/PropagatorData.h
@@ -42,7 +42,7 @@ struct PropagatorState
uint32_t nPropagations; ///< Predictions from last received NAS state
NASState nas;
- Eigen::Vector3f acceleration = Eigen::Vector3f::Zero();
+ Eigen::Vector3f acceleration = Eigen::Vector3f::Zero();
PropagatorState() : timestamp(0), nPropagations(0), nas() {}
@@ -54,7 +54,8 @@ struct PropagatorState
static std::string header()
{
- return "timestamp,nPropagations,n,e,d,vn,ve,vd,qx,qy,qz,qw,bx,by,bz\n";
+ return "timestamp,nPropagations,n,e,d,vn,ve,vd,qx,qy,qz,qw,bx,by,bz,ax,"
+ "ay,az\n";
}
void print(std::ostream& os) const
@@ -62,7 +63,9 @@ 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 << "\n";
+ << "," << nas.bx << "," << nas.by << "," << nas.bz << ","
+ << acceleration[0] << "," << acceleration[1] << ","
+ << acceleration[2] << "\n";
}
NASState getNasState() const { return nas; }
@@ -72,7 +75,10 @@ struct PropagatorState
*
* @return Eigen::Vector3f the NED position vector
*/
- Eigen::Vector3f getPosition() { return Eigen::Vector3f(nas.n, nas.e, nas.d); }
+ Eigen::Vector3f getPosition()
+ {
+ return Eigen::Vector3f(nas.n, nas.e, nas.d);
+ }
/**
* @brief Setter for the vector of positions NED
@@ -107,20 +113,14 @@ struct PropagatorState
/**
* @brief Setter for the vector acceleration
*/
- void setAcceleration(Eigen::Vector3f aProp)
- {
- acceleration = aProp;
- }
+ void setAcceleration(Eigen::Vector3f acc) { acceleration = acc; }
/**
- * @brief Getter for the vector acceleration
+ * @brief Getter for the vector acceleration
*
* @return Eigen::Vector3f acceleration
*/
- Eigen::Vector3f getAcceleration() const
- {
- return acceleration;
- }
+ Eigen::Vector3f getAcceleration() const { return acceleration; }
/**
* @brief Getter for the vector of quaternions
--
GitLab