Merge pull request #6235 from etracer65/throttle_angle_correction

Fix throttle angle correction when smoothing throttle; reduce processing overhead

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();
 
 }

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

@@ -126,6 +126,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
@@ -607,7 +609,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;
@@ -897,3 +899,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) {};
 }