diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 2e6d31943d..94c8098c71 100644 --- a/src/main/flight/imu.c +++ b/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(); } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e68eb111ae..cd681729c8 100644 --- a/src/main/flight/pid.c +++ b/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 diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c index 391ca4ec19..df8dc2a5b2 100644 --- a/src/main/osd/osd.c +++ b/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 } diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 57fdd6ac36..fd81054a28 100644 --- a/src/main/sensors/acceleration.c +++ b/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 diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index cd2de0f585..795ca88f56 100644 --- a/src/main/sensors/acceleration.h +++ b/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; diff --git a/src/main/sensors/acceleration_init.c b/src/main/sensors/acceleration_init.c index eb8bbf6e8e..62ce093395 100644 --- a/src/main/sensors/acceleration_init.c +++ b/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; } diff --git a/src/main/sensors/acceleration_init.h b/src/main/sensors/acceleration_init.h index 561741c22a..d40b10f0b4 100644 --- a/src/main/sensors/acceleration_init.h +++ b/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;