[NAV] Add accelerometer weight scale to DEBUG_VIBE

iNavFlight · May 6, 2019 · 0428b70 · 0428b70
1 parent 7598f22
commit 0428b70
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 2 deletions.
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
@@ -580,6 +580,7 @@ void imuCheckVibrationLevels(void)
     DEBUG_SET(DEBUG_VIBE, 1, accVibeLevels.y * 100);
     DEBUG_SET(DEBUG_VIBE, 2, accVibeLevels.z * 100);
     DEBUG_SET(DEBUG_VIBE, 3, accClipCount);
+    // DEBUG_VIBE values 4-7 are used by NAV estimator
 }
 
 void FAST_CODE NOINLINE imuUpdateAttitude(timeUs_t currentTimeUs)

diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
@@ -25,6 +25,7 @@
 #if defined(USE_NAV)
 
 #include "build/build_config.h"
+#include "build/debug.h"
 
 #include "common/axis.h"
 #include "common/log.h"
@@ -367,12 +368,17 @@ static void updateIMUEstimationWeight(const float dt)
         const float relAlpha = dt / (dt + INAV_ACC_CLIPPING_RC_CONSTANT);
         posEstimator.imu.accWeightFactor = posEstimator.imu.accWeightFactor * (1.0f - relAlpha) + 1.0f * relAlpha;
     }
+
+    // DEBUG_VIBE[0-3] are used in IMU
+    DEBUG_SET(DEBUG_VIBE, 4, posEstimator.imu.accWeightFactor * 1000);
 }
 
 float navGetAccelerometerWeight(void)
 {
-    // TODO(digitalentity): consider accelerometer health in weight calculation
-    return posEstimator.imu.accWeightFactor * positionEstimationConfig()->w_xyz_acc_p;
+    const float accWeightScaled = posEstimator.imu.accWeightFactor * positionEstimationConfig()->w_xyz_acc_p;
+    DEBUG_SET(DEBUG_VIBE, 5, accWeightScaled * 1000);
+
+    return accWeightScaled;
 }
 
 static void updateIMUTopic(timeUs_t currentTimeUs)