From 528d19b72ff5ec608020e59982bb0e2d40094306 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] [ARP] propagator with acceleration [ARP] chage of some names in PropagatorState structure [ARP] suggested changes [ARP]PropagatorData could not be logged, propagator algorithm improved [ARP] format [ARP] Last changes before internal merge Now only z-axis acceleration is used to propagate + some comments added. [ARP] Propagator max propagation time - Propagator: Changed a little the structure to have an overall maximum time for the propagation from the last received packet [ARP] Fixed max time propagation and acceleration - Acceleration (Propagator): Now the time to compute the acceleration is correctly converted to seconds (before it was wrongly on micros) - Fix max time (Propagator): The maximum propagation time was not applied due to wrong sign in the statement to check the propagation time [ARP] Fixed type issue with acceleration - Fixed error type for the acceleration: Since all types were uint, the casting was to uint for the acceleration. This caused infinite acceleration to be reached in case of times near to 0 seconds [ARP] Minor fixes for MR - Follwer: Added mutex for the init function, removed unused init variable - Propagator: Using now chrono for the MAX_ACCELERATION_TIME and to constexpr --- src/shared/algorithms/Follower/Follower.cpp | 3 +- src/shared/algorithms/Follower/Follower.h | 2 - .../algorithms/Propagator/Propagator.cpp | 53 +++++++++++++++++-- src/shared/algorithms/Propagator/Propagator.h | 13 +++++ .../algorithms/Propagator/PropagatorData.h | 41 +++++++++++--- 5 files changed, 96 insertions(+), 16 deletions(-) diff --git a/src/shared/algorithms/Follower/Follower.cpp b/src/shared/algorithms/Follower/Follower.cpp index 9f48adbc3..d4551e5bc 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 b161cf02e..ba7771d28 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 8fa10602d..b1d025e28 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 142fc41c2..ed0395a6e 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 228f306f0..154287f43 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 * -- GitLab