Merge pull request #3210 from iNavFlight/de_acc_vibre

Detect accelerometer 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

@@ -75,6 +75,9 @@ STATIC_FASTRAM int32_t accADC[XYZ_AXIS_COUNT];
 
 STATIC_FASTRAM biquadFilter_t accFilter[XYZ_AXIS_COUNT];
 
+STATIC_FASTRAM pt1Filter_t accVibeFloorFilter[XYZ_AXIS_COUNT];
+STATIC_FASTRAM pt1Filter_t accVibeFilter[XYZ_AXIS_COUNT];
+
 #ifdef USE_ACC_NOTCH
 STATIC_FASTRAM filterApplyFnPtr accNotchFilterApplyFn;
 STATIC_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT];
@@ -311,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) {
@@ -531,10 +535,27 @@ void accUpdate(void)
     applySensorAlignment(accADC, accADC, acc.dev.accAlign);
     applyBoardAlignment(accADC);
 
+    // Calculate acceleration readings in G's
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         acc.accADCf[axis] = (float)accADC[axis] / acc.dev.acc_1G;
     }
 
+    // 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) {
+        acc.accClipCount++;
+    }
+
+    // Calculate vibration levels
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        // filter accel at 5hz
+        const float accFloorFilt = pt1FilterApply(&accVibeFloorFilter[axis], acc.accADCf[axis]);
+
+        // 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);
+    }
+
+    // Filter acceleration
     if (accelerometerConfig()->acc_lpf_hz) {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             acc.accADCf[axis] = biquadFilterApply(&accFilter[axis], acc.accADCf[axis]);
@@ -554,6 +575,23 @@ void accUpdate(void)
 #endif
 }
 
+void accGetVibrationLevels(fpVector3_t *accVibeLevels)
+{
+    accVibeLevels->x = sqrtf(acc.accVibeSq[X]);
+    accVibeLevels->y = sqrtf(acc.accVibeSq[Y]);
+    accVibeLevels->z = sqrtf(acc.accVibeSq[Z]);
+}
+
+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) &&
@@ -573,6 +611,12 @@ void accInitFilters(void)
         }
     }
 
+    const float accDt = acc.accTargetLooptime * 1e-6f;
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        pt1FilterInit(&accVibeFloorFilter[axis], ACC_VIBE_FLOOR_FILT_HZ, accDt);
+        pt1FilterInit(&accVibeFilter[axis], ACC_VIBE_FILT_HZ, accDt);
+    }
+
 #ifdef USE_ACC_NOTCH
     STATIC_FASTRAM biquadFilter_t accFilterNotch[XYZ_AXIS_COUNT];
     accNotchFilterApplyFn = nullFilterApply;

src/main/sensors/acceleration.h

@@ -27,6 +27,10 @@
 #define GRAVITY_CMSS    980.665f
 #define GRAVITY_MSS     9.80665f
 
+#define ACC_CLIPPING_THRESHOLD_G        7.9f
+#define ACC_VIBE_FLOOR_FILT_HZ          5.0f
+#define ACC_VIBE_FILT_HZ                2.0f
+
 // Type of accelerometer used/detected
 typedef enum {
     ACC_NONE = 0,
@@ -48,6 +52,8 @@ typedef struct acc_s {
     accDev_t dev;
     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;
@@ -68,6 +74,9 @@ bool accInit(uint32_t accTargetLooptime);
 bool accIsCalibrationComplete(void);
 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);