From dd1fcd4d6ad397a393a632e9d3c35ee80ea912e7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Mon, 25 Nov 2024 15:35:09 +0100
Subject: [PATCH] [ARP] Propagator state log
Now PropagatorState is logged, the user-defined reference costructor and eigen vector3f were removed since logger needs trivially copiable structs
---
.../algorithms/Propagator/Propagator.cpp | 6 +-
src/shared/algorithms/Propagator/Propagator.h | 1 +
.../algorithms/Propagator/PropagatorData.h | 91 ++++++++++---------
3 files changed, 52 insertions(+), 46 deletions(-)
diff --git a/src/shared/algorithms/Propagator/Propagator.cpp b/src/shared/algorithms/Propagator/Propagator.cpp
index 634d1bf26..9076981b9 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 a8d43d4c0..142fc41c2 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 cab262da5..6fbd2aeb6 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);
}
};
--
GitLab