Ensure states are actually copied in non-error mode

1 files changed, 6 insertions, 0 deletions

diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp
index 5ac6b079f..89e137adc 100644
--- a/src/modules/ekf_att_pos_estimator/estimator.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator.cpp
@@ -2610,6 +2610,12 @@ void AttPosEKF::ZeroVariables()
 
 void AttPosEKF::GetFilterState(struct ekf_status_report *state)
 {
+
+    // Copy states
+    for (unsigned i = 0; i < n_states; i++) {
+        current_ekf_state.states[i] = states[i];
+    }
+
     memcpy(state, &current_ekf_state, sizeof(*state));
 }