diff --git a/src/shared/algorithms/Follower/Follower.cpp b/src/shared/algorithms/Follower/Follower.cpp
index 9f48adbc37bfc1359848496f060902ec10d1f6f2..d4551e5bc7fa7f0f3dc1c120ea76a63037a8c6da 100644
--- a/src/shared/algorithms/Follower/Follower.cpp
+++ b/src/shared/algorithms/Follower/Follower.cpp
@@ -129,8 +129,7 @@ bool Follower::setMaxGain(float yawGainNew, float pitchGainNew)
bool Follower::init()
{
- if (isInit)
- return true;
+ Lock<FastMutex> lock(followerMutex);
if (!antennaCoordinatesSet || !rocketCoordinatesSet)
{
LOG_ERR(logger, "Antenna or rocket coordinates not set");
diff --git a/src/shared/algorithms/Follower/Follower.h b/src/shared/algorithms/Follower/Follower.h
index b161cf02ea979fc339310679cd79e311c710c8d9..ba7771d2802386044dda9236c54a3448dc7d74ee 100644
--- a/src/shared/algorithms/Follower/Follower.h
+++ b/src/shared/algorithms/Follower/Follower.h
@@ -158,8 +158,6 @@ private:
// actuation update period [ms]
std::chrono::milliseconds updatePeriod;
- // Initialization flag
- std::atomic<bool> isInit{false};
// max number of retries for GPS data acquisition
const uint8_t maxInitRetries = 120;
diff --git a/src/shared/algorithms/Propagator/Propagator.cpp b/src/shared/algorithms/Propagator/Propagator.cpp
index 8fa10602dffb0ab12f8e11f5485cd8a857b98f91..b1d025e28077f9e5944ec8880e2bb7ea8f7ab274 100644
--- a/src/shared/algorithms/Propagator/Propagator.cpp
+++ b/src/shared/algorithms/Propagator/Propagator.cpp
@@ -31,6 +31,9 @@ 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()
{
@@ -43,18 +46,55 @@ 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
// last Propagator state
state = (oldState.nPropagations == 0
- ? PropagatorState(oldState.timestamp, oldState.nPropagations,
- getRocketNasState())
+ ? PropagatorState(
+ oldState.timestamp, oldState.nPropagations,
+ getRocketNasState()) // TODO: Uh? The timestamp? SUS
: oldState);
- // Update Position propagating it with velocity
- state.setXProp(state.getXProp() + state.getVProp() * updatePeriod);
- state.nPropagations++;
+ if (state.nPropagations == 0)
+ {
+ // Update the timestamp of the last received packet
+ lastReceivedTime = TimestampTimer::getTimestamp();
+
+ // Compute the acceleration to change the velocity
+ if (useAcceleration) // Update Position assuming constant acceleration
+ {
+ t1 = TimestampTimer::getTimestamp();
+ float dt = t1 - t0;
+ if (dt > 0 && dt < MAX_ACCELERATION_TIME.count() && t0 != 0)
+ state.setZAcceleration(
+ (state.getVelocity() - last_real_velocity) /
+ (dt / 1000000));
+ t0 = t1;
+ last_real_velocity = state.getVelocity();
+ }
+ }
+
+ // If we use the acceleration we update the velocity
+ if (useAcceleration &&
+ TimestampTimer::getTimestamp() < t0 + MAX_ACCELERATION_TIME.count())
+ state.setVelocity(state.getVelocity() +
+ state.getAcceleration() * updatePeriod);
+
+ // In case the time do not exceed the maximum propagation time or it is a
+ // new packet we update the state
+ if (TimestampTimer::getTimestamp() <
+ lastReceivedTime + MAX_PROPAGATION_TIME.count() ||
+ state.nPropagations == 0)
+ {
+ state.setPosition(state.getPosition() +
+ state.getVelocity() * updatePeriod);
+ state.nPropagations++;
+ }
+ else
+ state = oldState;
+
state.timestamp = TimestampTimer::getTimestamp();
// Log propagator state
@@ -70,6 +110,9 @@ void Propagator::setRocketNasState(const NASState& newRocketNasState)
// Reset nPropagations to notify another received "real" packet
state.nPropagations = 0;
lastRocketNasState = newRocketNasState;
+
+ // Using as timestamp ARP timestamp
+ state.timestamp = TimestampTimer::getTimestamp();
}
} // namespace Boardcore
diff --git a/src/shared/algorithms/Propagator/Propagator.h b/src/shared/algorithms/Propagator/Propagator.h
index 142fc41c21268b8abc42f209894bc95cfd23880b..ed0395a6e5c46dba618009402893e68d861b8292 100644
--- a/src/shared/algorithms/Propagator/Propagator.h
+++ b/src/shared/algorithms/Propagator/Propagator.h
@@ -35,6 +35,14 @@
namespace Boardcore
{
+static constexpr std::chrono::microseconds MAX_PROPAGATION_TIME =
+ std::chrono::seconds(5); ///< Max time for the propagation, to avoid
+ ///< infinite propagation [us]
+
+static constexpr std::chrono::microseconds MAX_ACCELERATION_TIME =
+ std::chrono::seconds(5); ///< Time limit after which acceleration data is
+ ///< ignored for propagation (5s)
+
/**
* @brief Predictor class that linearly propagates the last available rocket
* position by means of the rocket NAS velocity.
@@ -97,6 +105,11 @@ 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; ///< last non-propagated velocitylast_real_velocity
+ uint64_t t0 = 0, t1 = 0; ///< time values used to compute acceleration [u]
+ uint64_t lastReceivedTime =
+ 0; ///< Last timestamp from a non-propagated packet
};
} // namespace Boardcore
diff --git a/src/shared/algorithms/Propagator/PropagatorData.h b/src/shared/algorithms/Propagator/PropagatorData.h
index 228f306f047a586811061fda71a2d51dbe16be1d..154287f43aa93a60c833137c498671559a83b6bc 100644
--- a/src/shared/algorithms/Propagator/PropagatorData.h
+++ b/src/shared/algorithms/Propagator/PropagatorData.h
@@ -38,10 +38,13 @@ namespace Boardcore
*/
struct PropagatorState
{
- uint64_t timestamp; ///< Prediction timestamp [ms]
+ uint64_t timestamp; ///< Prediction timestamp (ARP timestamp) [ms]
uint32_t nPropagations; ///< Predictions from last received NAS state
NASState nas;
+ float az = 0;
+ static constexpr float ax = 0,
+ ay = 0; ///< only az is used by the propagator
PropagatorState() : timestamp(0), nPropagations(0), nas() {}
PropagatorState(uint64_t timestamp, uint32_t nPropagations,
@@ -52,7 +55,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
@@ -60,7 +64,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 << "\n";
+ << "," << nas.bx << "," << nas.by << "," << nas.bz << "," << ax
+ << "," << ay << "," << az << "\n";
}
NASState getNasState() const { return nas; }
@@ -70,12 +75,15 @@ struct PropagatorState
*
* @return Eigen::Vector3f the NED position vector
*/
- Eigen::Vector3f getXProp() { 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
*/
- void setXProp(Eigen::Vector3f xProp)
+ void setPosition(Eigen::Vector3f xProp)
{
nas.n = xProp(0);
nas.e = xProp(1);
@@ -87,7 +95,7 @@ struct PropagatorState
*
* @return Eigen::Vector3f the NED velocities vector
*/
- Eigen::Vector3f getVProp()
+ Eigen::Vector3f getVelocity()
{
return Eigen::Vector3f(nas.vn, nas.ve, nas.vd);
}
@@ -95,13 +103,32 @@ struct PropagatorState
/**
* @brief Setter for the vector of velocities NED
*/
- void setVProp(Eigen::Vector3f vProp)
+ void setVelocity(Eigen::Vector3f vProp)
{
nas.vn = vProp(0);
nas.ve = vProp(1);
nas.vd = vProp(2);
}
+ /**
+ * @brief Setter for the vector acceleration(only z-axis)
+ */
+ void setZAcceleration(Eigen::Vector3f acc) { az = acc(2); }
+
+ /**
+ * @brief Getter for the vector acceleration
+ *
+ * @return Eigen::Vector3f 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
*