Fix bug that prevented throttle angle correction working correctly when

using different values in different.

The cause was the IMU init code which triggered calculation was never
called after switching profiles - it couldn't be called twice because it
also initialised the compass.

The solution was to decouple compass initialisation from IMU
initialisation and extract the code to recalculate throttle angle scale
to a new method.

src/main/config/config.c

@@ -382,6 +382,8 @@ void activateConfig(void)
             &currentProfile.gimbalConfig
             );
 
+    calculateThrottleAngleScale(currentProfile.throttle_correction_angle);
+
 #ifdef BARO
     useBarometerConfig(&currentProfile.barometerConfig);
 #endif

src/main/config/config_profile.h

@@ -49,6 +49,7 @@ typedef struct profile_s {
     uint8_t yaw_deadband;                    // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
     uint8_t alt_hold_throttle_neutral;      // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40
     uint8_t alt_hold_fast_change;           // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_throttle_neutral; when enabled, altitude changes slowly proportional to stick movement
+
     uint16_t throttle_correction_angle;     // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
     uint8_t throttle_correction_value;      // the correction that will be applied at throttle_correction_angle.
 

src/main/flight/imu.c

@@ -73,7 +73,6 @@ int32_t errorAltitudeI = 0;
 
 int32_t vario = 0;                      // variometer in cm/s
 
-int16_t throttleAngleCorrection = 0;    // correction of throttle in lateral wind,
 float throttleAngleScale;
 
 int32_t BaroPID = 0;
@@ -92,18 +91,16 @@ float anglerad[2] = { 0.0f, 0.0f };    // absolute angle inclination in radians
 
 static void getEstimatedAttitude(void);
 
-void imuInit(void)
+void imuInit()
 {
     smallAngle = lrintf(acc_1G * cosf(RAD * 25.0f));
     accVelScale = 9.80665f / acc_1G / 10000.0f;
-    throttleAngleScale = (1800.0f / M_PI) * (900.0f / currentProfile.throttle_correction_angle);
     gyroScaleRad = gyro.scale * (M_PI / 180.0f) * 0.000001f;
+}
 
-#ifdef MAG
-    // if mag sensor is enabled, use it
-    if (sensors(SENSOR_MAG))
-        compassInit();
-#endif
+void calculateThrottleAngleScale(uint16_t throttle_correction_angle)
+{
+    throttleAngleScale = (1800.0f / M_PI) * (900.0f / throttle_correction_angle);
 }
 
 void computeIMU(void)
@@ -353,21 +350,20 @@ static void getEstimatedAttitude(void)
         heading = calculateHeading(&EstN);
 
     acc_calc(deltaT); // rotate acc vector into earth frame
+}
 
-    if (currentProfile.throttle_correction_value) {
-
-        float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);
-
-        if (cosZ <= 0.015f) { // we are inverted, vertical or with a small angle < 0.86 deg
-            throttleAngleCorrection = 0;
-        } else {
-            int angle = lrintf(acosf(cosZ) * throttleAngleScale);
-            if (angle > 900)
-                angle = 900;
-            throttleAngleCorrection = lrintf(currentProfile.throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f))) ;
-        }
+// correction of throttle in lateral wind,
+int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
+{
+    float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);
 
+    if (cosZ <= 0.015f) { // we are inverted, vertical or with a small angle < 0.86 deg
+        return 0;
     }
+    int angle = lrintf(acosf(cosZ) * throttleAngleScale);
+    if (angle > 900)
+        angle = 900;
+    return lrintf(throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f)));
 }
 
 #ifdef BARO

src/main/flight/imu.h

@@ -23,3 +23,5 @@ extern int16_t throttleAngleCorrection;
 
 int getEstimatedAltitude(void);
 void computeIMU(void);
+void calculateThrottleAngleScale(uint16_t throttle_correction_angle);
+int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);

src/main/main.c

@@ -159,7 +159,12 @@ void init(void)
     LED0_OFF;
     LED1_OFF;
 
-    imuInit(); // Mag is initialized inside imuInit
+    imuInit();
+#ifdef MAG
+    if (sensors(SENSOR_MAG))
+        compassInit();
+#endif
+
     mixerInit(masterConfig.mixerConfiguration, masterConfig.customMixer);
 
     timerInit();

src/main/mw.c

@@ -606,7 +606,7 @@ void loop(void)
 #endif
 
         if (currentProfile.throttle_correction_value && (f.ANGLE_MODE || f.HORIZON_MODE)) {
-            rcCommand[THROTTLE] += throttleAngleCorrection;
+            rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile.throttle_correction_value);
         }
 
         if (sensors(SENSOR_GPS)) {