Debug vibration

src/main/build/debug.h

@@ -61,6 +61,7 @@ typedef enum {
     DEBUG_STAGE2,
     DEBUG_WIND_ESTIMATOR,
     DEBUG_SAG_COMP_VOLTAGE,
+    DEBUG_VIBE,
     DEBUG_COUNT
 } debugType_e;
 

src/main/fc/settings.yaml

@@ -67,7 +67,7 @@ tables:
   - name: i2c_speed
     values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"]
   - name: debug_modes
-    values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE"]
+    values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE", "VIBE"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator

src/main/flight/imu.c

@@ -548,6 +548,19 @@ void imuUpdateAccelerometer(void)
 #endif
 }
 
+void imuCheckVibrationLevels(void)
+{
+    fpVector3_t accVibeLevels;
+
+    accGetVibrationLevels(&accVibeLevels);
+    const uint32_t accClipCount = accGetClipCount();
+
+    DEBUG_SET(DEBUG_VIBE, 0, accVibeLevels.x * 100);
+    DEBUG_SET(DEBUG_VIBE, 1, accVibeLevels.y * 100);
+    DEBUG_SET(DEBUG_VIBE, 2, accVibeLevels.z * 100);
+    DEBUG_SET(DEBUG_VIBE, 3, accClipCount);
+}
+
 void imuUpdateAttitude(timeUs_t currentTimeUs)
 {
     /* Calculate dT */
@@ -560,6 +573,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
         if (!hilActive) {
             gyroGetMeasuredRotationRate(&imuMeasuredRotationBF);    // Calculate gyro rate in body frame in rad/s
             accGetMeasuredAcceleration(&imuMeasuredAccelBF);  // Calculate accel in body frame in cm/s/s
+            imuCheckVibrationLevels();
             imuCalculateEstimatedAttitude(dT);  // Update attitude estimate
         }
         else {
@@ -569,6 +583,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
 #else
         gyroGetMeasuredRotationRate(&imuMeasuredRotationBF);    // Calculate gyro rate in body frame in rad/s
         accGetMeasuredAcceleration(&imuMeasuredAccelBF);  // Calculate accel in body frame in cm/s/s
+        imuCheckVibrationLevels();
         imuCalculateEstimatedAttitude(dT);  // Update attitude estimate
 #endif
     } else {

src/main/sensors/acceleration.c

@@ -314,6 +314,7 @@ bool accInit(uint32_t targetLooptime)
     acc.dev.acc_1G = 256; // set default
     acc.dev.initFn(&acc.dev);
     acc.accTargetLooptime = targetLooptime;
+    acc.accClipCount = 0;
     accInitFilters();
 
     if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) {
@@ -541,7 +542,7 @@ void accUpdate(void)
 
     // Before filtering check for clipping and vibration levels
     if (ABS(acc.accADCf[X]) > ACC_CLIPPING_THRESHOLD_G || ABS(acc.accADCf[Y]) > ACC_CLIPPING_THRESHOLD_G || ABS(acc.accADCf[Z]) > ACC_CLIPPING_THRESHOLD_G) {
-        debug[3]++;
+        acc.accClipCount++;
     }
 
     // Calculate vibration levels
@@ -552,8 +553,6 @@ void accUpdate(void)
         // calc difference from this sample and 5hz filtered value, square and filter at 2hz
         const float accDiff = acc.accADCf[axis] - accFloorFilt;
         acc.accVibeSq[axis] = pt1FilterApply(&accVibeFilter[axis], accDiff * accDiff);
-
-        debug[axis] = acc.accVibeSq[axis] * 100;
     }
 
     // Filter acceleration
@@ -588,6 +587,11 @@ float accGetVibrationLevel(void)
     return sqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
 }
 
+uint32_t accGetClipCount(void)
+{
+    return acc.accClipCount;
+}
+
 void accSetCalibrationValues(void)
 {
     if ((accelerometerConfig()->accZero.raw[X] == 0) && (accelerometerConfig()->accZero.raw[Y] == 0) && (accelerometerConfig()->accZero.raw[Z] == 0) &&

src/main/sensors/acceleration.h

@@ -53,6 +53,7 @@ typedef struct acc_s {
     uint32_t accTargetLooptime;
     float accADCf[XYZ_AXIS_COUNT]; // acceleration in g
     float accVibeSq[XYZ_AXIS_COUNT];
+    uint32_t accClipCount;
 } acc_t;
 
 extern acc_t acc;
@@ -75,6 +76,7 @@ void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
 void accGetMeasuredAcceleration(fpVector3_t *measuredAcc);
 void accGetVibrationLevels(fpVector3_t *accVibeLevels);
 float accGetVibrationLevel(void);
+uint32_t accGetClipCount(void);
 void accUpdate(void);
 void accSetCalibrationValues(void);
 void accInitFilters(void);