Collision Detection (#13010)

src/main/flight/imu.c

@@ -732,9 +732,10 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
         mixerSetThrottleAngleCorrection(throttleAngleCorrection);
 
     } else {
-        acc.accADC.x = 0;
-        acc.accADC.y = 0;
-        acc.accADC.z = 0;
+        vector3Zero(&acc.accADC);
+        vector3Zero(&acc.jerk);
+        acc.accMagnitude = 0.0f;
+        acc.jerkMagnitude = 0.0f;
         schedulerIgnoreTaskStateTime();
     }
 

src/main/flight/pid.c

@@ -939,8 +939,8 @@ static FAST_CODE_NOINLINE void disarmOnImpact(void)
 #ifdef USE_ALT_HOLD_MODE
         lowAltitudeSensitivity = (FLIGHT_MODE(ALT_HOLD_MODE) && isBelowLandingAltitude()) ? 1.5f : 1.0f;
 #endif
-        // and disarm if accelerometer jerk exceeds threshold...
-        if ((fabsf(acc.accDelta) * lowAltitudeSensitivity) > pidRuntime.landingDisarmThreshold) {
+        // and disarm if jerk exceeds threshold...
+        if ((acc.jerkMagnitude * lowAltitudeSensitivity) > pidRuntime.landingDisarmThreshold) {
             // then disarm
             setArmingDisabled(ARMING_DISABLED_ARM_SWITCH); // NB: need a better message
             disarm(DISARM_REASON_LANDING);
@@ -948,7 +948,7 @@ static FAST_CODE_NOINLINE void disarmOnImpact(void)
         }
     }
     DEBUG_SET(DEBUG_EZLANDING, 6, lrintf(getMaxRcDeflectionAbs() * 100.0f));
-    DEBUG_SET(DEBUG_EZLANDING, 7, lrintf(acc.accDelta));
+    DEBUG_SET(DEBUG_EZLANDING, 7, lrintf(acc.jerkMagnitude * 1e3f));
 }
 
 #ifdef USE_LAUNCH_CONTROL

src/main/osd/osd.c

@@ -1314,7 +1314,7 @@ void osdProcessStats3(void)
     if (sensors(SENSOR_ACC)
        && (VISIBLE(osdElementConfig()->item_pos[OSD_G_FORCE]) || osdStatGetState(OSD_STAT_MAX_G_FORCE))) {
             // only calculate the G force if the element is visible or the stat is enabled
-        osdGForce = vector3Norm(&acc.accADC) * acc.dev.acc_1G_rec;
+        osdGForce = acc.accMagnitude;
     }
 #endif
 }

src/main/sensors/acceleration.c

@@ -28,7 +28,6 @@
 
 #include "build/debug.h"
 
-#include "common/axis.h"
 #include "common/filter.h"
 #include "common/utils.h"
 
@@ -36,58 +35,93 @@
 
 #include "sensors/acceleration_init.h"
 #include "sensors/boardalignment.h"
+#include "sensors/gyro.h"
 
 #include "acceleration.h"
 
 FAST_DATA_ZERO_INIT acc_t acc;                       // acc access functions
 
-static void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrims)
+static inline void alignAccelerometer(void)
 {
-    acc.accADC.x -= accelerationTrims->raw[X];
-    acc.accADC.y -= accelerationTrims->raw[Y];
-    acc.accADC.z -= accelerationTrims->raw[Z];
+    switch (acc.dev.accAlign) {
+        case ALIGN_CUSTOM:
+            alignSensorViaMatrix(&acc.accADC, &acc.dev.rotationMatrix);
+            break;
+        default:
+            alignSensorViaRotation(&acc.accADC, acc.dev.accAlign);
+            break;
+    }
 }
 
-void accUpdate(timeUs_t currentTimeUs)
+static inline void calibrateAccelerometer(void)
 {
-    UNUSED(currentTimeUs);
-    static float previousAccMagnitude;
-
-    if (!acc.dev.readFn(&acc.dev)) {
-        return;
-    }
-    acc.isAccelUpdatedAtLeastOnce = true;
-
-    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        const int16_t val =  acc.dev.ADCRaw[axis];
-        acc.accADC.v[axis] = val;
-    }
-
-    if (acc.dev.accAlign == ALIGN_CUSTOM) {
-        alignSensorViaMatrix(&acc.accADC, &acc.dev.rotationMatrix);
-    } else {
-        alignSensorViaRotation(&acc.accADC, acc.dev.accAlign);
-    }
-
     if (!accIsCalibrationComplete()) {
+        // acc.accADC is held at 0 until calibration is completed
         performAcclerationCalibration(&accelerometerConfigMutable()->accelerometerTrims);
     }
 
     if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL)) {
         performInflightAccelerationCalibration(&accelerometerConfigMutable()->accelerometerTrims);
     }
-
-    applyAccelerationTrims(accelerationRuntime.accelerationTrims);
-
-    float accAdcSquaredSum = 0.0f;
-    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        const float val = acc.accADC.v[axis];
-        acc.accADC.v[axis] = accelerationRuntime.accLpfCutHz ? pt2FilterApply(&accelerationRuntime.accFilter[axis], val) : val;
-        accAdcSquaredSum += sq(acc.accADC.v[axis]);
-    }
-    acc.accMagnitude = sqrtf(accAdcSquaredSum) * acc.dev.acc_1G_rec; // normally 1.0; used for disarm on impact detection
-    acc.accDelta = (acc.accMagnitude - previousAccMagnitude) * acc.sampleRateHz;
-    previousAccMagnitude = acc.accMagnitude;
 }
 
-#endif
+static inline void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrims)
+{
+    acc.accADC.x -= accelerationTrims->raw[X];
+    acc.accADC.y -= accelerationTrims->raw[Y];
+    acc.accADC.z -= accelerationTrims->raw[Z];
+}
+
+static inline void postProcessAccelerometer(void)
+{
+    static vector3_t accAdcPrev;
+
+    for (unsigned axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+
+        // Apply anti-alias filter for attitude task (if enabled)
+        if (axis == gyro.gyroDebugAxis) {
+            DEBUG_SET(DEBUG_ACCELEROMETER, 0, lrintf(acc.accADC.v[axis]));
+        }
+
+        if (accelerationRuntime.accLpfCutHz) {
+            acc.accADC.v[axis] = pt2FilterApply(&accelerationRuntime.accFilter[axis], acc.accADC.v[axis]);
+        }
+
+        // Calculate derivative of acc (jerk)
+        acc.jerk.v[axis] = (acc.accADC.v[axis] - accAdcPrev.v[axis]) * acc.sampleRateHz;
+        accAdcPrev.v[axis] = acc.accADC.v[axis];
+
+        if (axis == gyro.gyroDebugAxis) {
+            DEBUG_SET(DEBUG_ACCELEROMETER, 1, lrintf(acc.accADC.v[axis]));
+            DEBUG_SET(DEBUG_ACCELEROMETER, 3, lrintf(acc.jerk.v[axis] * 1e-2f));
+        }
+    }
+
+    acc.accMagnitude = vector3Norm(&acc.accADC) * acc.dev.acc_1G_rec;
+    acc.jerkMagnitude = vector3Norm(&acc.jerk) * acc.dev.acc_1G_rec;
+
+    DEBUG_SET(DEBUG_ACCELEROMETER, 2, lrintf(acc.accMagnitude * 1e3f));
+    DEBUG_SET(DEBUG_ACCELEROMETER, 4, lrintf(acc.jerkMagnitude * 1e3f));
+}
+
+void accUpdate(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+
+    if (!acc.dev.readFn(&acc.dev)) {
+        return;
+    }
+
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        acc.accADC.v[axis] = acc.dev.ADCRaw[axis];
+    }
+
+    alignAccelerometer();
+    calibrateAccelerometer();
+    applyAccelerationTrims(accelerationRuntime.accelerationTrims);
+    postProcessAccelerometer();
+
+    acc.isAccelUpdatedAtLeastOnce = true;
+}
+
+#endif // USE_ACC

src/main/sensors/acceleration.h

@@ -20,9 +20,14 @@
 
 #pragma once
 
+#include "common/axis.h"
 #include "common/time.h"
-#include "pg/pg.h"
+#include "common/vector.h"
+
 #include "drivers/accgyro/accgyro.h"
+
+#include "pg/pg.h"
+
 #include "sensors/sensors.h"
 
 // Type of accelerometer used/detected
@@ -54,10 +59,11 @@ typedef enum {
 typedef struct acc_s {
     accDev_t dev;
     uint16_t sampleRateHz;
-    vector3_t accADC;
+    vector3_t accADC;                       // rotated but unscaled ADC value
+    vector3_t jerk;
+    float accMagnitude;                     // in multiples of 1G
+    float jerkMagnitude;                    // in multiples of 1G/s (measure of collision strength)
     bool isAccelUpdatedAtLeastOnce;
-    float accMagnitude;
-    float accDelta;
 } acc_t;
 
 extern acc_t acc;
@@ -74,7 +80,7 @@ typedef union rollAndPitchTrims_u {
 
 #if defined(USE_ACC)
 typedef struct accelerometerConfig_s {
-    uint16_t acc_lpf_hz;                    // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
+    uint16_t acc_lpf_hz;                    // cutoff frequency for attitude anti-aliasing filter
     uint8_t acc_hardware;                   // Which acc hardware to use on boards with more than one device
     bool acc_high_fsr;
     flightDynamicsTrims_t accZero;

src/main/sensors/acceleration_init.c

@@ -29,8 +29,6 @@
 
 #include "build/debug.h"
 
-#include "common/axis.h"
-#include "common/filter.h"
 #include "common/utils.h"
 
 #include "config/config_reset.h"
@@ -69,9 +67,10 @@
 #include "drivers/accgyro/legacy/accgyro_mma845x.h"
 #endif
 
+#include "config/config.h"
+
 #include "drivers/bus_spi.h"
 
-#include "config/config.h"
 #include "fc/runtime_config.h"
 
 #include "io/beeper.h"
@@ -83,7 +82,6 @@
 #include "sensors/boardalignment.h"
 #include "sensors/gyro.h"
 #include "sensors/gyro_init.h"
-#include "sensors/sensors.h"
 
 #include "acceleration_init.h"
 
@@ -377,7 +375,7 @@ void accInitFilters(void)
     // the filter initialization is not defined (sample rate = 0)
     accelerationRuntime.accLpfCutHz = (acc.sampleRateHz) ? accelerometerConfig()->acc_lpf_hz : 0;
     if (accelerationRuntime.accLpfCutHz) {
-        const float k = pt2FilterGain(accelerationRuntime.accLpfCutHz, 1.0f / acc.sampleRateHz);
+        const float k = pt2FilterGain(accelerationRuntime.accLpfCutHz, HZ_TO_INTERVAL(acc.sampleRateHz));
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             pt2FilterInit(&accelerationRuntime.accFilter[axis], k);
         }
@@ -401,7 +399,6 @@ bool accInit(uint16_t accSampleRateHz)
 #ifdef USE_MULTI_GYRO
     if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2) {
         alignment = gyroDeviceConfig(1)->alignment;
-
         customAlignment = &gyroDeviceConfig(1)->customAlignment;
     }
 #endif
@@ -417,6 +414,7 @@ bool accInit(uint16_t accSampleRateHz)
 
     acc.sampleRateHz = accSampleRateHz;
     accInitFilters();
+
     return true;
 }
 

src/main/sensors/acceleration_init.h

@@ -22,7 +22,11 @@
 
 #include "platform.h"
 
+#include "common/axis.h"
+#include "common/filter.h"
+
 #include "sensors/acceleration.h"
+#include "sensors/sensors.h"
 
 typedef struct accelerationRuntime_s {
     uint16_t accLpfCutHz;