Fix throttle angle correction when smoothing throttle; reduce processing overhead

Change the logic to not modify rcCommand directly and instead apply the additional throttle directly in the mixer.

Also move the logic to the attitude task instead of having it calculate in the PID loop. The logic relies on an angle that's only updated in the attitude task so there was no point in running the calculation every PID loop.

src/main/fc/config.c

@@ -136,7 +136,7 @@ static void activateConfig(void)
     setAccelerationTrims(&accelerometerConfigMutable()->accZero);
     accInitFilters();
 
-    imuConfigure(throttleCorrectionConfig()->throttle_correction_angle);
+    imuConfigure(throttleCorrectionConfig()->throttle_correction_angle, throttleCorrectionConfig()->throttle_correction_value);
 #endif // USE_OSD_SLAVE
 
 #ifdef USE_LED_STRIP

src/main/fc/fc_core.c

@@ -986,10 +986,6 @@ static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs)
         resetYawAxis();
     }
 
-    if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
-        rcCommand[THROTTLE] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->throttle_correction_value);
-    }
-
     processRcCommand();
 
 #if defined(USE_GPS_RESCUE)

src/main/flight/imu.c

@@ -92,6 +92,7 @@ float accVelScale;
 bool canUseGPSHeading = true;
 
 static float throttleAngleScale;
+static int throttleAngleValue;
 static float fc_acc;
 static float smallAngleCosZ = 0;
 
@@ -153,7 +154,7 @@ static float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
     return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
 }
 
-void imuConfigure(uint16_t throttle_correction_angle)
+void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correction_value)
 {
     imuRuntimeConfig.dcm_kp = imuConfig()->dcm_kp / 10000.0f;
     imuRuntimeConfig.dcm_ki = imuConfig()->dcm_ki / 10000.0f;
@@ -162,6 +163,8 @@ void imuConfigure(uint16_t throttle_correction_angle)
 
     fc_acc = calculateAccZLowPassFilterRCTimeConstant(5.0f); // Set to fix value
     throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
+    
+    throttleAngleValue = throttle_correction_value;
 }
 
 void imuInit(void)
@@ -503,6 +506,22 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
 #endif
 }
 
+int calculateThrottleAngleCorrection(void)
+{
+    /*
+    * Use 0 as the throttle angle correction if we are inverted, vertical or with a
+    * small angle < 0.86 deg
+    * TODO: Define this small angle in config.
+    */
+    if (rMat[2][2] <= 0.015f) {
+        return 0;
+    }
+    int angle = lrintf(acos_approx(rMat[2][2]) * throttleAngleScale);
+    if (angle > 900)
+        angle = 900;
+    return lrintf(throttleAngleValue * sin_approx(angle / (900.0f * M_PIf / 2.0f)));
+}
+
 void imuUpdateAttitude(timeUs_t currentTimeUs)
 {
     if (sensors(SENSOR_ACC) && acc.isAccelUpdatedAtLeastOnce) {
@@ -516,6 +535,14 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
 #endif
         imuCalculateEstimatedAttitude(currentTimeUs);
         IMU_UNLOCK;
+        
+        // Update the throttle correction for angle and supply it to the mixer
+        int throttleAngleCorrection = 0;
+        if (throttleAngleValue && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && ARMING_FLAG(ARMED)) {
+            throttleAngleCorrection = calculateThrottleAngleCorrection();
+        }
+        mixerSetThrottleAngleCorrection(throttleAngleCorrection);
+
     } else {
         acc.accADC[X] = 0;
         acc.accADC[Y] = 0;
@@ -549,22 +576,6 @@ void getQuaternion(quaternion *quat)
    quat->z = q.z;
 }
 
-int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
-{
-    /*
-    * Use 0 as the throttle angle correction if we are inverted, vertical or with a
-    * small angle < 0.86 deg
-    * TODO: Define this small angle in config.
-    */
-    if (rMat[2][2] <= 0.015f) {
-        return 0;
-    }
-    int angle = lrintf(acos_approx(rMat[2][2]) * throttleAngleScale);
-    if (angle > 900)
-        angle = 900;
-    return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f)));
-}
-
 void imuComputeQuaternionFromRPY(quaternionProducts *quatProd, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw)
 {
     if (initialRoll > 1800) {

src/main/flight/imu.h

@@ -78,12 +78,11 @@ typedef struct imuRuntimeConfig_s {
     accDeadband_t accDeadband;
 } imuRuntimeConfig_t;
 
-void imuConfigure(uint16_t throttle_correction_angle);
+void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correction_value);
 
 float getCosTiltAngle(void);
 void getQuaternion(quaternion * q);
 void imuUpdateAttitude(timeUs_t currentTimeUs);
-int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
 
 void imuResetAccelerationSum(void);
 void imuInit(void);

src/main/flight/mixer.c

@@ -125,6 +125,8 @@ float motor_disarmed[MAX_SUPPORTED_MOTORS];
 mixerMode_e currentMixerMode;
 static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
 
+static FAST_RAM_ZERO_INIT int throttleAngleCorrection;
+
 
 static const motorMixer_t mixerQuadX[] = {
     { 1.0f, -1.0f,  1.0f, -1.0f },          // REAR_R
@@ -606,7 +608,7 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
             pidResetITerm();
         }
     } else {
-        throttle = rcCommand[THROTTLE] - rxConfig()->mincheck;
+        throttle = rcCommand[THROTTLE] - rxConfig()->mincheck + throttleAngleCorrection;
         currentThrottleInputRange = rcCommandThrottleRange;
         motorRangeMin = motorOutputLow;
         motorRangeMax = motorOutputHigh;
@@ -888,3 +890,8 @@ uint16_t convertMotorToExternal(float motorValue)
 
     return externalValue;
 }
+
+void mixerSetThrottleAngleCorrection(int correctionValue)
+{
+    throttleAngleCorrection = correctionValue;
+}

src/main/flight/mixer.h

@@ -134,3 +134,5 @@ void stopPwmAllMotors(void);
 float convertExternalToMotor(uint16_t externalValue);
 uint16_t convertMotorToExternal(float motorValue);
 bool mixerIsTricopter(void);
+
+void mixerSetThrottleAngleCorrection(int correctionValue);

src/test/unit/flight_imu_unittest.cc

@@ -243,4 +243,5 @@ void performBaroCalibrationCycle(void) {}
 int32_t baroCalculateAltitude(void) { return 0; }
 bool gyroGetAccumulationAverage(float *) { return false; }
 bool accGetAccumulationAverage(float *) { return false; }
+void mixerSetThrottleAngleCorrection(int) {};
 }