From 33f98abf8046506e071845a78d6341b6c8749cf0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Jun 2014 20:16:17 +0200 Subject: [PATCH 1/2] Fill error report --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 89e6d3948c..28eab49f22 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2266,7 +2266,7 @@ int AttPosEKF::CheckAndBound() // Reset the filter if gyro offsets are excessive if (fabs(states[10]) > 1.0f || fabsf(states[11]) > 1.0f || fabsf(states[12]) > 1.0f) { - + FillErrorReport(&last_ekf_error); InitializeDynamic(velNED, magDeclination); // that's all we can do here, return From 2219dd3fc6586e7da8a1bc7b7514b97261ff8b23 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Jun 2014 20:16:41 +0200 Subject: [PATCH 2/2] Undo hacking --- src/modules/ekf_att_pos_estimator/estimator_21states.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp index 0f5a17a335..67bfec4ea3 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp @@ -61,8 +61,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED() // Apply corrections for earths rotation rate and coning errors // * and + operators have been overloaded - correctedDelAng = dAngIMU;//correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); - correctedDelAng.z = 0; + correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); // Convert the rotation vector to its equivalent quaternion rotationMag = correctedDelAng.length(); if (rotationMag < 1e-12f)