Refactoring of IMU and ACC

src/main/build/debug.c

@@ -102,5 +102,6 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "RX_STATE_TIME",
     "GPS_RESCUE_VELOCITY",
     "GPS_RESCUE_HEADING",
-    "GPS_RESCUE_TRACKING"
+    "GPS_RESCUE_TRACKING",
+    "ATTITUDE",
 };

src/main/build/debug.h

@@ -101,6 +101,7 @@ typedef enum {
     DEBUG_GPS_RESCUE_VELOCITY,
     DEBUG_GPS_RESCUE_HEADING,
     DEBUG_GPS_RESCUE_TRACKING,
+    DEBUG_ATTITUDE,
     DEBUG_COUNT
 } debugType_e;
 

src/main/cli/settings.c

@@ -709,7 +709,7 @@ const clivalue_t valueTable[] = {
 #if defined(USE_GYRO_SPI_ICM20649)
     { "acc_high_range",             VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_high_fsr) },
 #endif
-    { PARAM_NAME_ACC_LPF_HZ,        VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 400 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_lpf_hz) },
+    { PARAM_NAME_ACC_LPF_HZ,        VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_lpf_hz) },
     { "acc_trim_pitch",             VAR_INT16  | MASTER_VALUE, .config.minmax = { -300, 300 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accelerometerTrims.values.pitch) },
     { "acc_trim_roll",              VAR_INT16  | MASTER_VALUE, .config.minmax = { -300, 300 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accelerometerTrims.values.roll) },
 
@@ -1000,6 +1000,7 @@ const clivalue_t valueTable[] = {
     { "imu_dcm_kp",                 VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 32000 }, PG_IMU_CONFIG, offsetof(imuConfig_t, dcm_kp) },
     { "imu_dcm_ki",                 VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 32000 }, PG_IMU_CONFIG, offsetof(imuConfig_t, dcm_ki) },
     { "small_angle",                VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 180 }, PG_IMU_CONFIG, offsetof(imuConfig_t, small_angle) },
+    { "imu_process_denom",          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 1, 4 }, PG_IMU_CONFIG, offsetof(imuConfig_t, imu_process_denom) },
 
 // PG_ARMING_CONFIG
     { "auto_disarm_delay",          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 60 }, PG_ARMING_CONFIG, offsetof(armingConfig_t, auto_disarm_delay) },

src/main/fc/core.c

@@ -996,10 +996,10 @@ void processRxModes(timeUs_t currentTimeUs)
     if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
         LED1_ON;
         // increase frequency of attitude task to reduce drift when in angle or horizon mode
-        rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(500));
+        rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(acc.sampleRateHz / (float)imuConfig()->imu_process_denom));
     } else {
         LED1_OFF;
-        rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(100));
+        rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(acc.sampleRateHz / 10.0f));
     }
 
     if (!IS_RC_MODE_ACTIVE(BOXPREARM) && ARMING_FLAG(WAS_ARMED_WITH_PREARM)) {

src/main/fc/tasks.c

@@ -161,7 +161,7 @@ static void taskBatteryAlerts(timeUs_t currentTimeUs)
 #ifdef USE_ACC
 static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
 {
-    accUpdate(currentTimeUs, &accelerometerConfigMutable()->accelerometerTrims);
+    accUpdate(currentTimeUs);
 }
 #endif
 

src/main/flight/imu.c

@@ -88,13 +88,10 @@ static bool imuUpdated = false;
 #define GPS_COG_MIN_GROUNDSPEED 200        // 200cm/s minimum groundspeed for a gps based IMU heading to be considered valid
                                            // Better to have some update than none for GPS Rescue at slow return speeds
 
-float accAverage[XYZ_AXIS_COUNT];
-
 bool canUseGPSHeading = true;
 
 static float throttleAngleScale;
 static int throttleAngleValue;
-static float fc_acc;
 static float smallAngleCosZ = 0;
 
 static imuRuntimeConfig_t imuRuntimeConfig;
@@ -113,12 +110,13 @@ quaternion offset = QUATERNION_INITIALIZE;
 // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
 attitudeEulerAngles_t attitude = EULER_INITIALIZE;
 
-PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 1);
+PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 2);
 
 PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
     .dcm_kp = 2500,                // 1.0 * 10000
     .dcm_ki = 0,                   // 0.003 * 10000
     .small_angle = 25,
+    .imu_process_denom = 2
 );
 
 static void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd)
@@ -156,14 +154,6 @@ STATIC_UNIT_TESTED void imuComputeRotationMatrix(void){
 #endif
 }
 
-/*
-* Calculate RC time constant used in the accZ lpf.
-*/
-static float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
-{
-    return 0.5f / (M_PIf * accz_lpf_cutoff);
-}
-
 static float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
 {
     return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
@@ -176,7 +166,6 @@ void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correctio
 
     smallAngleCosZ = cos_approx(degreesToRadians(imuConfig()->small_angle));
 
-    fc_acc = calculateAccZLowPassFilterRCTimeConstant(5.0f); // Set to fix value
     throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
 
     throttleAngleValue = throttle_correction_value;
@@ -501,16 +490,10 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
 #if defined(USE_GPS)
     if (!useMag && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= GPS_COG_MIN_GROUNDSPEED) {
         // Use GPS course over ground to correct attitude.values.yaw
-        if (isFixedWing()) {
+        courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
-            courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
+        useCOG = true;
-            useCOG = true;
-        } else {
-            courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
 
-            useCOG = true;
+        if (shouldInitializeGPSHeading()) {
-        }
-
-        if (useCOG && shouldInitializeGPSHeading()) {
             // Reset our reference and reinitialize quaternion.  This will likely ideally happen more than once per flight, but for now,
             // shouldInitializeGPSHeading() returns true only once.
             imuComputeQuaternionFromRPY(&qP, attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
@@ -537,15 +520,15 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
     deltaT = imuDeltaT;
 #endif
     float gyroAverage[XYZ_AXIS_COUNT];
-    gyroGetAccumulationAverage(gyroAverage);
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; ++axis) {
-
+        gyroAverage[axis] = gyroGetFilteredDownsampled(axis);
-    if (accGetAccumulationAverage(accAverage)) {
-        useAcc = imuIsAccelerometerHealthy(accAverage);
     }
 
+    useAcc = imuIsAccelerometerHealthy(acc.accADC);
+
     imuMahonyAHRSupdate(deltaT * 1e-6f,
                         DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]),
-                        useAcc, accAverage[X], accAverage[Y], accAverage[Z],
+                        useAcc, acc.accADC[X], acc.accADC[Y], acc.accADC[Z],
                         useMag,
                         useCOG, courseOverGround,  imuCalcKpGain(currentTimeUs, useAcc, gyroAverage));
 
@@ -596,6 +579,9 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
         acc.accADC[Z] = 0;
         schedulerIgnoreTaskStateTime();
     }
+
+    DEBUG_SET(DEBUG_ATTITUDE, X, acc.accADC[X]);
+    DEBUG_SET(DEBUG_ATTITUDE, Y, acc.accADC[Y]);
 }
 #endif // USE_ACC
 

src/main/flight/imu.h

@@ -27,7 +27,6 @@
 
 // Exported symbols
 extern bool canUseGPSHeading;
-extern float accAverage[XYZ_AXIS_COUNT];
 
 typedef struct {
     float w,x,y,z;
@@ -57,6 +56,7 @@ typedef struct imuConfig_s {
     uint16_t dcm_kp;                        // DCM filter proportional gain ( x 10000)
     uint16_t dcm_ki;                        // DCM filter integral gain ( x 10000)
     uint8_t small_angle;
+    uint8_t imu_process_denom;
 } imuConfig_t;
 
 PG_DECLARE(imuConfig_t, imuConfig);

src/main/flight/pid.c

@@ -369,7 +369,7 @@ static float getLevelModeRcDeflection(uint8_t axis)
 }
 
 // calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
-STATIC_UNIT_TESTED float calcHorizonLevelStrength(void)
+STATIC_UNIT_TESTED FAST_CODE_NOINLINE float calcHorizonLevelStrength()
 {
     // start with 1.0 at center stick, 0.0 at max stick deflection:
     float horizonLevelStrength = 1.0f - MAX(fabsf(getLevelModeRcDeflection(FD_ROLL)), fabsf(getLevelModeRcDeflection(FD_PITCH)));
@@ -421,29 +421,33 @@ STATIC_UNIT_TESTED float calcHorizonLevelStrength(void)
             horizonLevelStrength = ((horizonLevelStrength - 1) * (100 / sensitFact)) + 1;
         }
     }
+
     return constrainf(horizonLevelStrength, 0, 1);
 }
 
 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM to avoid overflow.
 // The impact is possibly slightly slower performance on F7/H7 but they have more than enough
 // processing power that it should be a non-issue.
-STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
+STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim,
+                                                        float currentPidSetpoint, float horizonLevelStrength) {
+    const float levelAngleLimit = pidProfile->levelAngleLimit;
     // calculate error angle and limit the angle to the max inclination
     // rcDeflection is in range [-1.0, 1.0]
-    float angle = pidProfile->levelAngleLimit * getLevelModeRcDeflection(axis);
+    float angle = levelAngleLimit * getLevelModeRcDeflection(axis);
 #ifdef USE_GPS_RESCUE
     angle += gpsRescueAngle[axis] / 100; // ANGLE IS IN CENTIDEGREES
 #endif
-    angle = constrainf(angle, -pidProfile->levelAngleLimit, pidProfile->levelAngleLimit);
+    angle = constrainf(angle, -levelAngleLimit, levelAngleLimit);
     const float errorAngle = angle - ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f);
     if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) {
         // ANGLE mode - control is angle based
-        currentPidSetpoint = errorAngle * pidRuntime.levelGain;
+        const float setpointCorrection = errorAngle * pidRuntime.levelGain;
+        currentPidSetpoint = pt3FilterApply(&pidRuntime.attitudeFilter[axis], setpointCorrection);
     } else {
         // HORIZON mode - mix of ANGLE and ACRO modes
         // mix in errorAngle to currentPidSetpoint to add a little auto-level feel
-        const float horizonLevelStrength = calcHorizonLevelStrength();
+        const float setpointCorrection = errorAngle * pidRuntime.horizonGain * horizonLevelStrength;
-        currentPidSetpoint = currentPidSetpoint + (errorAngle * pidRuntime.horizonGain * horizonLevelStrength);
+        currentPidSetpoint += pt3FilterApply(&pidRuntime.attitudeFilter[axis], setpointCorrection);
     }
     return currentPidSetpoint;
 }
@@ -821,18 +825,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
     static float previousGyroRateDterm[XYZ_AXIS_COUNT];
     static float previousRawGyroRateDterm[XYZ_AXIS_COUNT];
 
-#if defined(USE_ACC)
-    static timeUs_t levelModeStartTimeUs = 0;
-    static bool gpsRescuePreviousState = false;
-#endif
-
-#if defined(USE_ACC)
-    const rollAndPitchTrims_t *angleTrim = &accelerometerConfig()->accelerometerTrims;
-#else
-    UNUSED(pidProfile);
-    UNUSED(currentTimeUs);
-#endif
-
 #ifdef USE_TPA_MODE
     const float tpaFactorKp = (currentControlRateProfile->tpaMode == TPA_MODE_PD) ? pidRuntime.tpaFactor : 1.0f;
 #else
@@ -846,6 +838,11 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
     const bool launchControlActive = isLaunchControlActive();
 
 #if defined(USE_ACC)
+    static timeUs_t levelModeStartTimeUs = 0;
+    static bool gpsRescuePreviousState = false;
+    const rollAndPitchTrims_t *angleTrim = &accelerometerConfig()->accelerometerTrims;
+    float horizonLevelStrength = 0.0f;
+
     const bool gpsRescueIsActive = FLIGHT_MODE(GPS_RESCUE_MODE);
     levelMode_e levelMode;
     if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || gpsRescueIsActive) {
@@ -854,23 +851,30 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
         } else {
             levelMode = LEVEL_MODE_RP;
         }
-    } else {
-        levelMode = LEVEL_MODE_OFF;
-    }
 
-    // Keep track of when we entered a self-level mode so that we can
+        // Keep track of when we entered a self-level mode so that we can
-    // add a guard time before crash recovery can activate.
+        // add a guard time before crash recovery can activate.
-    // Also reset the guard time whenever GPS Rescue is activated.
+        // Also reset the guard time whenever GPS Rescue is activated.
-    if (levelMode) {
         if ((levelModeStartTimeUs == 0) || (gpsRescueIsActive && !gpsRescuePreviousState)) {
             levelModeStartTimeUs = currentTimeUs;
         }
+
+        // Calc horizonLevelStrength if needed
+        if (FLIGHT_MODE(HORIZON_MODE)) {
+            horizonLevelStrength = calcHorizonLevelStrength();
+        }
     } else {
+        levelMode = LEVEL_MODE_OFF;
         levelModeStartTimeUs = 0;
     }
+
     gpsRescuePreviousState = gpsRescueIsActive;
+#else
+    UNUSED(pidProfile);
+    UNUSED(currentTimeUs);
 #endif
 
+
     // Dynamic i component,
     if ((pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) && pidRuntime.antiGravityEnabled) {
         // traditional itermAccelerator factor for iTerm
@@ -887,7 +891,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
     }
     DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(pidRuntime.itermAccelerator * 1000));
 
-    float agGain = pidRuntime.dT * pidRuntime.itermAccelerator * AG_KI;
+    const float agGain = pidRuntime.dT * pidRuntime.itermAccelerator * AG_KI;
 
     // gradually scale back integration when above windup point
     float dynCi = pidRuntime.dT;
@@ -906,15 +910,12 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
         // This is done to avoid DTerm spikes that occur with dynamically
         // calculated deltaT whenever another task causes the PID
         // loop execution to be delayed.
-        const float delta =
-            - (gyroRateDterm[axis] - previousRawGyroRateDterm[axis]) * pidRuntime.pidFrequency / D_LPF_RAW_SCALE;
         previousRawGyroRateDterm[axis] = gyroRateDterm[axis];
 
-        // Log the unfiltered D
+        // Log the unfiltered D for ROLL and PITCH
-        if (axis == FD_ROLL) {
+        if (axis != FD_YAW) {
-            DEBUG_SET(DEBUG_D_LPF, 0, lrintf(delta));
+            const float delta = (previousRawGyroRateDterm[axis] - gyroRateDterm[axis]) * pidRuntime.pidFrequency / D_LPF_RAW_SCALE;
-        } else if (axis == FD_PITCH) {
+            DEBUG_SET(DEBUG_D_LPF, axis, lrintf(delta));
-            DEBUG_SET(DEBUG_D_LPF, 1, lrintf(delta));
         }
 
         gyroRateDterm[axis] = pidRuntime.dtermNotchApplyFn((filter_t *) &pidRuntime.dtermNotch[axis], gyroRateDterm[axis]);
@@ -929,10 +930,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
 #endif
 
 #ifdef USE_FEEDFORWARD
-    bool newRcFrame = false;
+    const bool newRcFrame = getShouldUpdateFeedforward();
-    if (getShouldUpdateFeedforward()) {
-        newRcFrame = true;
-    }
 #endif
 
     // ----------PID controller----------
@@ -945,21 +943,10 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
         // Yaw control is GYRO based, direct sticks control is applied to rate PID
         // When Race Mode is active PITCH control is also GYRO based in level or horizon mode
 #if defined(USE_ACC)
-        switch (levelMode) {
+        if ((levelMode == LEVEL_MODE_R && axis == FD_ROLL)
-        case LEVEL_MODE_OFF:
+            || (levelMode == LEVEL_MODE_RP && (axis == FD_ROLL || axis ==  FD_PITCH)) ) {
-
+            currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint, horizonLevelStrength);
-            break;
+            DEBUG_SET(DEBUG_ATTITUDE, axis - FD_ROLL + 2, currentPidSetpoint);
-        case LEVEL_MODE_R:
-            if (axis == FD_PITCH) {
-                break;
-            }
-
-            FALLTHROUGH;
-        case LEVEL_MODE_RP:
-            if (axis == FD_YAW) {
-                break;
-            }
-            currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
         }
 #endif
 
@@ -999,7 +986,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
         const float previousIterm = pidData[axis].I;
         float itermErrorRate = errorRate;
 #ifdef USE_ABSOLUTE_CONTROL
-        float uncorrectedSetpoint = currentPidSetpoint;
+        const float uncorrectedSetpoint = currentPidSetpoint;
 #endif
 
 #if defined(USE_ITERM_RELAX)
@@ -1009,7 +996,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
         }
 #endif
 #ifdef USE_ABSOLUTE_CONTROL
-        float setpointCorrection = currentPidSetpoint - uncorrectedSetpoint;
+        const float setpointCorrection = currentPidSetpoint - uncorrectedSetpoint;
 #endif
 
         // --------low-level gyro-based PID based on 2DOF PID controller. ----------
@@ -1092,18 +1079,13 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
             // Log the value of D pre application of TPA
             preTpaD *= D_LPF_FILT_SCALE;
 
-            if (axis == FD_ROLL) {
+            if (axis != FD_YAW) {
-                DEBUG_SET(DEBUG_D_LPF, 2, lrintf(preTpaD));
+                DEBUG_SET(DEBUG_D_LPF, axis - FD_ROLL + 2, lrintf(preTpaD));
-            } else if (axis == FD_PITCH) {
-                DEBUG_SET(DEBUG_D_LPF, 3, lrintf(preTpaD));
             }
         } else {
             pidData[axis].D = 0;
-
+            if (axis != FD_YAW) {
-            if (axis == FD_ROLL) {
+                DEBUG_SET(DEBUG_D_LPF, axis - FD_ROLL + 2, 0);
-                DEBUG_SET(DEBUG_D_LPF, 2, 0);
-            } else if (axis == FD_PITCH) {
-                DEBUG_SET(DEBUG_D_LPF, 3, 0);
             }
         }
 
@@ -1176,7 +1158,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
         const float agBoost = 1.0f + (pidRuntime.antiGravityPBoost / agBoostAttenuator);
         if (axis != FD_YAW) {
             pidData[axis].P *= agBoost;
-            DEBUG_SET(DEBUG_ANTI_GRAVITY, axis + 2, lrintf(agBoost * 1000));
+            DEBUG_SET(DEBUG_ANTI_GRAVITY, axis - FD_ROLL + 2, lrintf(agBoost * 1000));
         }
 
         const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F;
@@ -1277,8 +1259,7 @@ void dynLpfDTermUpdate(float throttle)
 
 float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo) {
     const float expof = expo / 10.0f;
-    static float curve;
+    const float curve = throttle * (1 - throttle) * expof + throttle;
-    curve = throttle * (1 - throttle) * expof + throttle;
     return (dynLpfMax - dynLpfMin) * curve + dynLpfMin;
 }
 

src/main/flight/pid.h

@@ -391,6 +391,10 @@ typedef struct pidRuntime_s {
     float feedforwardJitterFactor;
     float feedforwardBoostFactor;
 #endif
+
+#ifdef USE_ACC
+    pt3Filter_t attitudeFilter[2];  // Only for ROLL and PITCH
+#endif
 } pidRuntime_t;
 
 extern pidRuntime_t pidRuntime;
@@ -437,8 +441,8 @@ void applyItermRelax(const int axis, const float iterm,
 void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate);
 void rotateItermAndAxisError();
 float pidLevel(int axis, const pidProfile_t *pidProfile,
-    const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint);
+    const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint, float horizonLevelStrength);
-float calcHorizonLevelStrength(void);
+float calcHorizonLevelStrength();
 #endif
 void dynLpfDTermUpdate(float throttle);
 void pidSetItermReset(bool enabled);

src/main/flight/pid_init.c

@@ -54,6 +54,7 @@
 
 #define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15  // The anti gravity throttle highpass filter cutoff
 #define ANTI_GRAVITY_SMOOTH_FILTER_CUTOFF 3  // The anti gravity P smoothing filter cutoff
+#define ATTITUDE_CUTOFF_HZ 250
 
 static void pidSetTargetLooptime(uint32_t pidLooptime)
 {
@@ -238,6 +239,13 @@ void pidInitFilters(const pidProfile_t *pidProfile)
 
     pt1FilterInit(&pidRuntime.antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, pidRuntime.dT));
     pt1FilterInit(&pidRuntime.antiGravitySmoothLpf, pt1FilterGain(ANTI_GRAVITY_SMOOTH_FILTER_CUTOFF, pidRuntime.dT));
+
+#ifdef USE_ACC
+    const float k = pt3FilterGain(ATTITUDE_CUTOFF_HZ, pidRuntime.dT);
+    for (int axis = 0; axis < 2; axis++) {  // ROLL and PITCH only
+        pt3FilterInit(&pidRuntime.attitudeFilter[axis], k);
+    }
+#endif
 }
 
 void pidInit(const pidProfile_t *pidProfile)

src/main/osd/osd.c

@@ -1122,7 +1122,7 @@ void osdProcessStats3()
        && (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
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            const float a = accAverage[axis];
+            const float a = acc.accADC[axis];
             osdGForce += a * a;
         }
         osdGForce = sqrtf(osdGForce) * acc.dev.acc_1G_rec;

src/main/sensors/acceleration.c

@@ -20,6 +20,7 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <math.h>
 
 #include "platform.h"
 
@@ -47,7 +48,7 @@ static void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrim
     acc.accADC[Z] -= accelerationTrims->raw[Z];
 }
 
-void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims)
+void accUpdate(timeUs_t currentTimeUs)
 {
     UNUSED(currentTimeUs);
 
@@ -57,14 +58,8 @@ void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims)
     acc.isAccelUpdatedAtLeastOnce = true;
 
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        DEBUG_SET(DEBUG_ACCELEROMETER, axis, acc.dev.ADCRaw[axis]);
+        const int16_t val =  acc.dev.ADCRaw[axis];
-        acc.accADC[axis] = acc.dev.ADCRaw[axis];
+        acc.accADC[axis] = val;
-    }
-
-    if (accelerationRuntime.accLpfCutHz) {
-        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            acc.accADC[axis] = biquadFilterApply(&accelerationRuntime.accFilter[axis], acc.accADC[axis]);
-        }
     }
 
     if (acc.dev.accAlign == ALIGN_CUSTOM) {
@@ -74,36 +69,18 @@ void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims)
     }
 
     if (!accIsCalibrationComplete()) {
-        performAcclerationCalibration(rollAndPitchTrims);
+        performAcclerationCalibration(&accelerometerConfigMutable()->accelerometerTrims);
     }
 
     if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL)) {
-        performInflightAccelerationCalibration(rollAndPitchTrims);
+        performInflightAccelerationCalibration(&accelerometerConfigMutable()->accelerometerTrims);
     }
 
     applyAccelerationTrims(accelerationRuntime.accelerationTrims);
 
-    ++accelerationRuntime.accumulatedMeasurementCount;
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        accelerationRuntime.accumulatedMeasurements[axis] += acc.accADC[axis];
+        const int16_t val = acc.accADC[axis];
-    }
+        acc.accADC[axis] = accelerationRuntime.accLpfCutHz ? pt2FilterApply(&accelerationRuntime.accFilter[axis], val) : val;
-}
-
-bool accGetAccumulationAverage(float *accumulationAverage)
-{
-    if (accelerationRuntime.accumulatedMeasurementCount > 0) {
-        // If we have gyro data accumulated, calculate average rate that will yield the same rotation
-        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            accumulationAverage[axis] = accelerationRuntime.accumulatedMeasurements[axis] / accelerationRuntime.accumulatedMeasurementCount;
-            accelerationRuntime.accumulatedMeasurements[axis] = 0.0f;
-        }
-        accelerationRuntime.accumulatedMeasurementCount = 0;
-        return true;
-    } else {
-        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            accumulationAverage[axis] = 0.0f;
-        }
-        return false;
     }
 }
 

src/main/sensors/acceleration.h

@@ -86,8 +86,7 @@ bool accIsCalibrationComplete(void);
 bool accHasBeenCalibrated(void);
 void accStartCalibration(void);
 void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims);
-void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims);
+void accUpdate(timeUs_t currentTimeUs);
-bool accGetAccumulationAverage(float *accumulation);
 union flightDynamicsTrims_u;
 void setAccelerationTrims(union flightDynamicsTrims_u *accelerationTrimsToUse);
 void accInitFilters(void);

src/main/sensors/acceleration_init.c

@@ -124,7 +124,7 @@ static void resetFlightDynamicsTrims(flightDynamicsTrims_t *accZero)
 void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
 {
     RESET_CONFIG_2(accelerometerConfig_t, instance,
-        .acc_lpf_hz = 10,
+        .acc_lpf_hz = 25, // ATTITUDE/IMU runs at 100Hz (acro) or 500Hz (level modes) so we need to set 50 Hz (or lower) to avoid aliasing
         .acc_hardware = ACC_DEFAULT,
         .acc_high_fsr = false,
     );
@@ -352,9 +352,9 @@ 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 uint32_t accSampleTimeUs = 1e6 / acc.sampleRateHz;
+        const float k = pt2FilterGain(accelerationRuntime.accLpfCutHz, 1.0f / acc.sampleRateHz);
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            biquadFilterInitLPF(&accelerationRuntime.accFilter[axis], accelerationRuntime.accLpfCutHz, accSampleTimeUs);
+            pt2FilterInit(&accelerationRuntime.accFilter[axis], k);
         }
     }
 }

src/main/sensors/acceleration_init.h

@@ -27,10 +27,8 @@
 
 typedef struct accelerationRuntime_s {
     uint16_t accLpfCutHz;
-    biquadFilter_t accFilter[XYZ_AXIS_COUNT];
+    pt2Filter_t accFilter[XYZ_AXIS_COUNT];
     flightDynamicsTrims_t *accelerationTrims;
-    int accumulatedMeasurementCount;
-    float accumulatedMeasurements[XYZ_AXIS_COUNT];
     uint16_t calibratingA;      // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
 } accelerationRuntime_t;
 

src/main/sensors/gyro.c

@@ -77,9 +77,7 @@ static FAST_DATA_ZERO_INIT bool yawSpinDetected;
 static FAST_DATA_ZERO_INIT timeUs_t yawSpinTimeUs;
 #endif
 
-static FAST_DATA_ZERO_INIT float accumulatedMeasurements[XYZ_AXIS_COUNT];
+static FAST_DATA_ZERO_INIT float gyroFilteredDownsampled[XYZ_AXIS_COUNT];
-static FAST_DATA_ZERO_INIT float gyroPrevious[XYZ_AXIS_COUNT];
-static FAST_DATA_ZERO_INIT int accumulatedMeasurementCount;
 
 static FAST_DATA_ZERO_INIT int16_t gyroSensorTemperature;
 
@@ -526,11 +524,8 @@ FAST_CODE void gyroFiltering(timeUs_t currentTimeUs)
 
     if (!overflowDetected) {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            // integrate using trapezium rule to avoid bias
+            gyroFilteredDownsampled[axis] = pt1FilterApply(&gyro.imuGyroFilter[axis], gyro.gyroADCf[axis]);
-            accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyro.gyroADCf[axis]) * gyro.targetLooptime;
-            gyroPrevious[axis] = gyro.gyroADCf[axis];
         }
-        accumulatedMeasurementCount++;
     }
 
 #if !defined(USE_GYRO_OVERFLOW_CHECK) && !defined(USE_YAW_SPIN_RECOVERY)
@@ -538,23 +533,9 @@ FAST_CODE void gyroFiltering(timeUs_t currentTimeUs)
 #endif
 }
 
-bool gyroGetAccumulationAverage(float *accumulationAverage)
+float gyroGetFilteredDownsampled(int axis)
 {
-    if (accumulatedMeasurementCount) {
+    return gyroFilteredDownsampled[axis];
-        // If we have gyro data accumulated, calculate average rate that will yield the same rotation
-        const timeUs_t accumulatedMeasurementTimeUs = accumulatedMeasurementCount * gyro.targetLooptime;
-        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            accumulationAverage[axis] = accumulatedMeasurements[axis] / accumulatedMeasurementTimeUs;
-            accumulatedMeasurements[axis] = 0.0f;
-        }
-        accumulatedMeasurementCount = 0;
-        return true;
-    } else {
-        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            accumulationAverage[axis] = 0.0f;
-        }
-        return false;
-    }
 }
 
 int16_t gyroReadSensorTemperature(gyroSensor_t gyroSensor)

src/main/sensors/gyro.h

@@ -49,6 +49,8 @@
 #define YAW_SPIN_RECOVERY_THRESHOLD_MAX 1950
 #endif
 
+#define GYRO_IMU_DOWNSAMPLE_CUTOFF_HZ 200
+
 typedef union gyroLowpassFilter_u {
     pt1Filter_t pt1FilterState;
     biquadFilter_t biquadFilterState;
@@ -127,7 +129,7 @@ typedef struct gyro_s {
 #ifdef USE_GYRO_OVERFLOW_CHECK
     uint8_t overflowAxisMask;
 #endif
-
+    pt1Filter_t imuGyroFilter[XYZ_AXIS_COUNT];
 } gyro_t;
 
 extern gyro_t gyro;
@@ -202,7 +204,7 @@ PG_DECLARE(gyroConfig_t, gyroConfig);
 
 void gyroUpdate(void);
 void gyroFiltering(timeUs_t currentTimeUs);
-bool gyroGetAccumulationAverage(float *accumulation);
+float gyroGetFilteredDownsampled(int axis);
 void gyroStartCalibration(bool isFirstArmingCalibration);
 bool isFirstArmingGyroCalibrationRunning(void);
 bool gyroIsCalibrationComplete(void);

src/main/sensors/gyro_init.c

@@ -266,6 +266,11 @@ void gyroInitFilters(void)
 #ifdef USE_DYN_NOTCH_FILTER
     dynNotchInit(dynNotchConfig(), gyro.targetLooptime);
 #endif
+
+    const float k = pt1FilterGain(GYRO_IMU_DOWNSAMPLE_CUTOFF_HZ, gyro.targetLooptime);
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        pt1FilterInit(&gyro.imuGyroFilter[axis], k);
+    }
 }
 
 #if defined(USE_GYRO_SLEW_LIMITER)

src/test/unit/arming_prevention_unittest.cc

@@ -57,6 +57,7 @@ extern "C" {
     PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
     PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0);
     PG_REGISTER(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0);
+    PG_REGISTER(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0);
 
     float rcCommand[4];
     float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];

src/test/unit/flight_imu_unittest.cc

@@ -254,4 +254,5 @@ void schedulerIgnoreTaskExecTime(void) {}
 void schedulerIgnoreTaskStateTime(void) {}
 void schedulerSetNextStateTime(timeDelta_t) {}
 bool schedulerGetIgnoreTaskExecTime() { return false; }
+float gyroGetFilteredDownsampled(int) { return 0.0f; }
 }

src/test/unit/link_quality_unittest.cc

@@ -79,7 +79,6 @@ extern "C" {
     gpsSolutionData_t gpsSol;
     float motor[8];
     acc_t acc;
-    float accAverage[XYZ_AXIS_COUNT];
 
     PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0);
     PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);

src/test/unit/osd_unittest.cc

@@ -85,7 +85,6 @@ extern "C" {
     linkQualitySource_e linkQualitySource;
 
     acc_t acc;
-    float accAverage[XYZ_AXIS_COUNT];
 
     PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0);
     PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);

src/test/unit/pid_unittest.cc

@@ -375,39 +375,39 @@ TEST(pidControllerTest, testPidLevel) {
     float currentPidSetpoint = 30;
     rollAndPitchTrims_t angleTrim = { { 0, 0 } };
 
-    currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
+    currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
     EXPECT_FLOAT_EQ(0, currentPidSetpoint);
-    currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
+    currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
     EXPECT_FLOAT_EQ(0, currentPidSetpoint);
 
     // Test attitude response
     setStickPosition(FD_ROLL, 1.0f);
     setStickPosition(FD_PITCH, -1.0f);
-    currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
+    currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
-    EXPECT_FLOAT_EQ(275, currentPidSetpoint);
+    EXPECT_FLOAT_EQ(244.07211, currentPidSetpoint);
-    currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
+    currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
-    EXPECT_FLOAT_EQ(-275, currentPidSetpoint);
+    EXPECT_FLOAT_EQ(-244.07211, currentPidSetpoint);
 
     setStickPosition(FD_ROLL, -0.5f);
     setStickPosition(FD_PITCH, 0.5f);
-    currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
+    currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
-    EXPECT_FLOAT_EQ(-137.5, currentPidSetpoint);
+    EXPECT_FLOAT_EQ(-93.487915, currentPidSetpoint);
-    currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
+    currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
-    EXPECT_FLOAT_EQ(137.5, currentPidSetpoint);
+    EXPECT_FLOAT_EQ(93.487915, currentPidSetpoint);
 
     attitude.values.roll = -275;
     attitude.values.pitch = 275;
-    currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
+    currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
-    EXPECT_FLOAT_EQ(0, currentPidSetpoint);
+    EXPECT_FLOAT_EQ(-12.047981, currentPidSetpoint);
-    currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
+    currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
-    EXPECT_FLOAT_EQ(0, currentPidSetpoint);
+    EXPECT_FLOAT_EQ(12.047981, currentPidSetpoint);
 
     // Disable ANGLE_MODE
     disableFlightMode(ANGLE_MODE);
-    currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
+    currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
-    EXPECT_FLOAT_EQ(0, currentPidSetpoint);
+    EXPECT_FLOAT_EQ(11.07958, currentPidSetpoint);
-    currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
+    currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
-    EXPECT_FLOAT_EQ(0, currentPidSetpoint);
+    EXPECT_FLOAT_EQ(12.047981, currentPidSetpoint);
 
     // Test level mode expo
     enableFlightMode(ANGLE_MODE);
@@ -417,10 +417,10 @@ TEST(pidControllerTest, testPidLevel) {
     setStickPosition(FD_PITCH, -0.5f);
     currentControlRateProfile->levelExpo[FD_ROLL] = 50;
     currentControlRateProfile->levelExpo[FD_PITCH] = 26;
-    currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
+    currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
-    EXPECT_FLOAT_EQ(85.9375, currentPidSetpoint);
+    EXPECT_FLOAT_EQ(76.208672, currentPidSetpoint);
-    currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
+    currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint, calcHorizonLevelStrength());
-    EXPECT_FLOAT_EQ(-110.6875, currentPidSetpoint);
+    EXPECT_FLOAT_EQ(-98.175163, currentPidSetpoint);
 }
 
 
@@ -433,18 +433,18 @@ TEST(pidControllerTest, testPidHorizon) {
     // Test full stick response
     setStickPosition(FD_ROLL, 1.0f);
     setStickPosition(FD_PITCH, -1.0f);
-    EXPECT_FLOAT_EQ(0, calcHorizonLevelStrength());
+    EXPECT_FLOAT_EQ(0.0f, calcHorizonLevelStrength());
 
     // Expect full rate output on full stick
     // Test zero stick response
     setStickPosition(FD_ROLL, 0);
     setStickPosition(FD_PITCH, 0);
-    EXPECT_FLOAT_EQ(1, calcHorizonLevelStrength());
+    EXPECT_FLOAT_EQ(1.0f, calcHorizonLevelStrength());
 
     // Test small stick response
     setStickPosition(FD_ROLL, 0.1f);
     setStickPosition(FD_PITCH, -0.1f);
-    EXPECT_NEAR(0.82, calcHorizonLevelStrength(), calculateTolerance(0.82));
+    EXPECT_NEAR(0.811f, calcHorizonLevelStrength(), calculateTolerance(0.82));
 }
 
 TEST(pidControllerTest, testMixerSaturation) {

src/test/unit/vtx_unittest.cc

@@ -60,6 +60,7 @@ extern "C" {
     PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
     PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0);
     PG_REGISTER(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0);
+    PG_REGISTER(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0);
 
     float rcCommand[4];
     float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
@@ -75,6 +76,7 @@ extern "C" {
     bool cmsInMenu = false;
     float axisPID_P[3], axisPID_I[3], axisPID_D[3], axisPIDSum[3];
     rxRuntimeState_t rxRuntimeState = {};
+    acc_t acc;
 }
 
 uint32_t simulationFeatureFlags = 0;