diff --git a/src/shared/algorithms/Propagator/Propagator.cpp b/src/shared/algorithms/Propagator/Propagator.cpp index fa537373f0a919a42eed320c6f0d4f40265ce360..c9fe1f67aecbfc022300d3b157f865232ee1098b 100644 --- a/src/shared/algorithms/Propagator/Propagator.cpp +++ b/src/shared/algorithms/Propagator/Propagator.cpp @@ -61,9 +61,9 @@ void Propagator::step() { t1 = state.timestamp; dt = t1 - t0; - if (dt > 0 && dt < 4646476 && t0 != 0) - state.setAcceleration(state.getVelocity() - - last_real_velocity / dt); + if (dt > 0 && dt < maxAccelerationTime && t0 != 0) + state.setZAcceleration((state.getVelocity() - last_real_velocity) / + dt); t0 = t1; last_real_velocity = state.getVelocity(); } diff --git a/src/shared/algorithms/Propagator/Propagator.h b/src/shared/algorithms/Propagator/Propagator.h index 2f2b4860b9931c03fd18c77bb92e851cfb1f6631..24968096cacd0b0a30de59e70100835da6480c86 100644 --- a/src/shared/algorithms/Propagator/Propagator.h +++ b/src/shared/algorithms/Propagator/Propagator.h @@ -97,8 +97,12 @@ 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; - uint64_t t0 = 0, t1 = 0, dt = 0; + Eigen::Vector3f last_real_velocity; ///< last non-propagated velocity + uint64_t t0 = 0, t1 = 0, + dt = 0; ///< time values used to compute acceleration [u] + const uint64_t maxAccelerationTime = + 5000000; ///< Time limit after which acceleration data is ignored for + ///< propagation (5s) }; } // namespace Boardcore diff --git a/src/shared/algorithms/Propagator/PropagatorData.h b/src/shared/algorithms/Propagator/PropagatorData.h index 542d62e01d03ffc21b2a7bd5adb639690a65d293..cc1ff643f99db8c7a0ad12f7d1dac8278947f6af 100644 --- a/src/shared/algorithms/Propagator/PropagatorData.h +++ b/src/shared/algorithms/Propagator/PropagatorData.h @@ -42,10 +42,9 @@ struct PropagatorState uint32_t nPropagations; ///< Predictions from last received NAS state NASState nas; - float ax = 0, ay = 0, - az = 0; // propagater acceleration (Eigen::Vector3f could not be used - // because it is not trivially copyable) - + 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, @@ -112,14 +111,9 @@ struct PropagatorState } /** - * @brief Setter for the vector acceleration + * @brief Setter for the vector acceleration(only z-axis) */ - void setAcceleration(Eigen::Vector3f acc) - { - ax = acc[0]; - ay = acc[1]; - az = acc[2]; - } + void setZAcceleration(Eigen::Vector3f acc) { az = acc(2); } /** * @brief Getter for the vector acceleration @@ -129,9 +123,9 @@ struct PropagatorState Eigen::Vector3f getAcceleration() const { Eigen::Vector3f acc; - acc[0] = ax; - acc[1] = ay; - acc[2] = az; + acc(0) = ax; + acc(1) = ay; + acc(2) = az; return acc; }