diff --git a/src/shared/algorithms/Propagator/Propagator.cpp b/src/shared/algorithms/Propagator/Propagator.cpp index 634d1bf26fdeebcdd5dbb98ca5b023254c02d220..9076981b930bc85739afef672470ed42626992d6 100644 --- a/src/shared/algorithms/Propagator/Propagator.cpp +++ b/src/shared/algorithms/Propagator/Propagator.cpp @@ -52,9 +52,13 @@ void Propagator::step() : oldState); // Update Position propagating it with velocity - state.x_prop = state.x_prop + state.v_prop * updatePeriod; + state.setXProp(state.getXProp() + state.getVProp() * updatePeriod); state.nPropagations++; state.timestamp = TimestampTimer::getTimestamp(); + + // Log propagator state + PropagatorState logState(state); + Boardcore::Logger::getInstance().log(logState); } void Propagator::setRocketNasState(const NASState& newRocketNasState) diff --git a/src/shared/algorithms/Propagator/Propagator.h b/src/shared/algorithms/Propagator/Propagator.h index a8d43d4c0af70241f16a5e46c68aca5a41ea37d7..142fc41c21268b8abc42f209894bc95cfd23880b 100644 --- a/src/shared/algorithms/Propagator/Propagator.h +++ b/src/shared/algorithms/Propagator/Propagator.h @@ -23,6 +23,7 @@ #pragma once #include <algorithms/Algorithm.h> +#include <logger/Logger.h> #include <miosix.h> #include <chrono> diff --git a/src/shared/algorithms/Propagator/PropagatorData.h b/src/shared/algorithms/Propagator/PropagatorData.h index cab262da5bea6993482136dea1d1989c03f3bcd7..6fbd2aeb63f0fdb04c0724903a5c6983afb9526e 100644 --- a/src/shared/algorithms/Propagator/PropagatorData.h +++ b/src/shared/algorithms/Propagator/PropagatorData.h @@ -31,50 +31,22 @@ namespace Boardcore { /** - * @brief Struct containing the state of the propagator. Stores the timestamp of - * the propagation, the number of propagations since the last NAS rocket packet - * and the propagated NAS state already divided in significant vectors so that - * computations with the NAS state are easier. - * It also exposes a method to retrieve the propagated NAS state as a NASState - * structure. + * @brief State of the propagator, taking into account the prediction steps (0 + * if true NAS state) and the propagated NAS + * @note Can be logged. + * */ struct PropagatorState { uint64_t timestamp; ///< Prediction timestamp [ms] uint32_t nPropagations; ///< Predictions from last received NAS state - Eigen::Vector3f x_prop; ///< Position propagation state NED [m] - Eigen::Vector3f v_prop; ///< Speed propagation state NED [m] - Eigen::Vector4f q_prop; ///< Quaternion propagation (scalar last) - Eigen::Vector3f b_prop; ///< Gyroscope bias propagation - - PropagatorState() - : timestamp(0), nPropagations(0), x_prop(0, 0, 0), v_prop(0, 0, 0), - q_prop(0, 0, 0, 1), b_prop(0, 0, 0) - { - } + NASState nas; - PropagatorState(const PropagatorState& newState) - : PropagatorState(newState.timestamp, newState.nPropagations, - newState.x_prop, newState.v_prop, newState.q_prop, - newState.b_prop) - { - } - - PropagatorState(uint64_t timestamp, uint32_t nPropagations, - Eigen::Vector3f x_prop, Eigen::Vector3f v_prop, - Eigen::Vector4f q_prop, Eigen::Vector3f b_prop) - : timestamp(timestamp), nPropagations(nPropagations), x_prop(x_prop), - v_prop(v_prop), q_prop(q_prop), b_prop(b_prop) - { - } + PropagatorState() : timestamp(0), nPropagations(0), nas() {} PropagatorState(uint64_t timestamp, uint32_t nPropagations, NASState nasState) - : timestamp(timestamp), nPropagations(nPropagations), - x_prop(nasState.n, nasState.e, nasState.d), - v_prop(nasState.vn, nasState.ve, nasState.vd), - q_prop(nasState.qx, nasState.qy, nasState.qz, nasState.qw), - b_prop(nasState.bx, nasState.by, nasState.bz) + : timestamp(timestamp), nPropagations(nPropagations), nas(nasState) { } @@ -85,19 +57,48 @@ struct PropagatorState void print(std::ostream& os) const { - os << timestamp << "," << nPropagations << "," << x_prop(0) << "," - << x_prop(1) << "," << x_prop(2) << "," << v_prop(0) << "," - << v_prop(1) << "," << v_prop(2) << "," << q_prop(0) << "," - << q_prop(1) << "," << q_prop(2) << "," << q_prop(3) << "," - << b_prop(0) << "," << b_prop(1) << "," << b_prop(2) << "\n"; + 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"; } - NASState getNasState() const + NASState getNasState() const { return nas; } + + Eigen::Vector3f getXProp() { return Eigen::Vector3f(nas.n, nas.e, nas.d); } + void setXProp(Eigen::Vector3f xProp) + { + nas.n = xProp(0); + nas.e = xProp(1); + nas.d = xProp(2); + } + Eigen::Vector3f getVProp() + { + return Eigen::Vector3f(nas.vn, nas.ve, nas.vd); + } + void setVProp(Eigen::Vector3f vProp) + { + nas.vn = vProp(0); + nas.ve = vProp(1); + nas.vd = vProp(2); + } + Eigen::Vector4f getQProp() + { + return Eigen::Vector4f(nas.qx, nas.qy, nas.qz, nas.qw); + } + void setQProp(Eigen::Vector4f qProp) + { + nas.qx = qProp(0); + nas.qy = qProp(1); + nas.qz = qProp(2); + nas.qw = qProp(3); + } + Eigen::Vector3f getBProp() { return Eigen::Vector3f(nas.n, nas.e, nas.d); } + void setBProp(Eigen::Vector3f bProp) { - Eigen::Matrix<float, 13, 1> nasState; - // cppcheck-suppress constStatement - nasState << x_prop, v_prop, q_prop, b_prop; - return NASState(timestamp, nasState); + nas.bx = bProp(0); + nas.by = bProp(1); + nas.bz = bProp(2); } };