Measure accelerometer vibration and clipping

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];
@@ -531,10 +534,29 @@ 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) {
+        debug[3]++;
+    }
+
+    // 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);
+
+        debug[axis] = acc.accVibeSq[axis] * 100;
+    }
+
+    // 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 +576,18 @@ 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]);
+}
+
 void accSetCalibrationValues(void)
 {
     if ((accelerometerConfig()->accZero.raw[X] == 0) && (accelerometerConfig()->accZero.raw[Y] == 0) && (accelerometerConfig()->accZero.raw[Z] == 0) &&
@@ -573,6 +607,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,7 @@ typedef struct acc_s {
     accDev_t dev;
     uint32_t accTargetLooptime;
     float accADCf[XYZ_AXIS_COUNT]; // acceleration in g
+    float accVibeSq[XYZ_AXIS_COUNT];
 } acc_t;
 
 extern acc_t acc;
@@ -68,6 +73,8 @@ 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);
 void accUpdate(void);
 void accSetCalibrationValues(void);
 void accInitFilters(void);