first build

src/main/fc/fc_core.c

@@ -833,13 +833,25 @@ void FAST_CODE taskGyro(timeUs_t currentTimeUs) {
 #endif
 }
 
-static float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompensationStrength)
+static void applyThrottleTiltCompensation(void)
 {
-    if (throttleTiltCompensationStrength) {
+    if (STATE(MULTIROTOR)) {
-        float tiltCompFactor = 1.0f / constrainf(calculateCosTiltAngle(), 0.6f, 1.0f);  // max tilt about 50 deg
+        int16_t thrTiltCompStrength = 0;
-        return 1.0f + (tiltCompFactor - 1.0f) * (throttleTiltCompensationStrength / 100.f);
+
-    } else {
+        if (navigationRequiresThrottleTiltCompensation()) {
-        return 1.0f;
+            thrTiltCompStrength = 100;
+        }
+        else if (systemConfig()->throttle_tilt_compensation_strength && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
+            thrTiltCompStrength = systemConfig()->throttle_tilt_compensation_strength;
+        }
+
+        if (thrTiltCompStrength) {
+            const int throttleIdleValue = getThrottleIdleValue();
+            float tiltCompFactor = 1.0f / constrainf(calculateCosTiltAngle(), 0.6f, 1.0f);  // max tilt about 50 deg
+            tiltCompFactor = 1.0f + (tiltCompFactor - 1.0f) * (thrTiltCompStrength / 100.f);
+
+            rcCommand[THROTTLE] = setDesiredThrottle(throttleIdleValue + (rcCommand[THROTTLE] - throttleIdleValue) * tiltCompFactor, false);
+        }
     }
 }
 
@@ -891,26 +903,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
     applyWaypointNavigationAndAltitudeHold();
 
     // Apply throttle tilt compensation
-    if (!STATE(FIXED_WING_LEGACY)) {
+    applyThrottleTiltCompensation();
-        int16_t thrTiltCompStrength = 0;
-
-        if (navigationRequiresThrottleTiltCompensation()) {
-            thrTiltCompStrength = 100;
-        }
-        else if (systemConfig()->throttle_tilt_compensation_strength && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
-            thrTiltCompStrength = systemConfig()->throttle_tilt_compensation_strength;
-        }
-
-        if (thrTiltCompStrength) {
-            rcCommand[THROTTLE] = constrain(getThrottleIdleValue()
-                                            + (rcCommand[THROTTLE] - getThrottleIdleValue()) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength),
-                                            getThrottleIdleValue(),
-                                            motorConfig()->maxthrottle);
-        }
-    }
-    else {
-        // FIXME: throttle pitch comp for FW
-    }
 
 #ifdef USE_POWER_LIMITS
     powerLimiterApply(&rcCommand[THROTTLE]);

src/main/fc/rc_controls.c

@@ -51,6 +51,7 @@
 
 #include "flight/pid.h"
 #include "flight/failsafe.h"
+#include "flight/mixer.h"
 
 #include "io/gps.h"
 #include "io/beeper.h"
@@ -111,6 +112,17 @@ bool isRollPitchStickDeflected(uint8_t deadband)
     return (ABS(rcCommand[ROLL]) > deadband) || (ABS(rcCommand[PITCH]) > deadband);
 }
 
+uint16_t setDesiredThrottle(uint16_t throttle, bool allowMotorStop)
+{
+    const uint16_t throttleIdleValue = getThrottleIdleValue();
+
+    if (allowMotorStop && throttle < throttleIdleValue) {
+        ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
+        return throttle;
+    }
+    return constrain(throttle, throttleIdleValue, motorConfig()->maxthrottle);
+}
+
 throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e type)
 {
     int value = rxGetChannelValue(THROTTLE);    // THROTTLE_STATUS_TYPE_RC

src/main/fc/rc_controls.h

@@ -109,6 +109,7 @@ bool checkStickPosition(stickPositions_e stickPos);
 bool areSticksInApModePosition(uint16_t ap_mode);
 bool areSticksDeflected(void);
 bool isRollPitchStickDeflected(uint8_t deadband);
+uint16_t setDesiredThrottle(uint16_t throttle, bool allowMotorStop);
 throttleStatus_e calculateThrottleStatus(throttleStatusType_e type);
 int16_t throttleStickMixedValue(void);
 rollPitchStatus_e calculateRollPitchCenterStatus(void);

src/main/flight/failsafe.c

@@ -211,14 +211,6 @@ bool failsafeRequiresAngleMode(void)
            failsafeProcedureLogic[failsafeState.activeProcedure].forceAngleMode;
 }
 
-bool failsafeRequiresMotorStop(void)
-{
-    return failsafeState.active &&
-           failsafeState.activeProcedure == FAILSAFE_PROCEDURE_AUTO_LANDING &&
-           posControl.flags.estAltStatus < EST_USABLE &&
-           currentBatteryProfile->failsafe_throttle < getThrottleIdleValue();
-}
-
 void failsafeStartMonitoring(void)
 {
     failsafeState.monitoring = true;

src/main/flight/failsafe.h

@@ -170,7 +170,6 @@ void failsafeOnRxResume(void);
 bool failsafeMayRequireNavigationMode(void);
 void failsafeApplyControlInput(void);
 bool failsafeRequiresAngleMode(void);
-bool failsafeRequiresMotorStop(void);
 bool failsafeShouldApplyControlInput(void);
 bool failsafeBypassNavigation(void);
 void failsafeUpdateRcCommandValues(void);

src/main/flight/mixer.c

@@ -616,7 +616,7 @@ int16_t getThrottlePercent(bool useScaled)
 {
     int16_t thr = constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
     const int idleThrottle = getThrottleIdleValue();
-    
+
     if (useScaled) {
        thr = (thr - idleThrottle) * 100 / (motorConfig()->maxthrottle - idleThrottle);
     } else {
@@ -627,11 +627,7 @@ int16_t getThrottlePercent(bool useScaled)
 
 motorStatus_e getMotorStatus(void)
 {
-    if (failsafeRequiresMotorStop()) {
+    if (STATE(NAV_MOTOR_STOP_OR_IDLE)) {
-        return MOTOR_STOPPED_AUTO;
-    }
-
-    if (!failsafeIsActive() && STATE(NAV_MOTOR_STOP_OR_IDLE)) {
         return MOTOR_STOPPED_AUTO;
     }
 

src/main/navigation/navigation_fixedwing.c

@@ -646,7 +646,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
             isAutoThrottleManuallyIncreased = false;
         }
 
-        rcCommand[THROTTLE] = constrain(correctedThrottleValue, getThrottleIdleValue(), motorConfig()->maxthrottle);
+        rcCommand[THROTTLE] = setDesiredThrottle(correctedThrottleValue, false);
     }
 
 #ifdef NAV_FIXED_WING_LANDING
@@ -661,7 +661,6 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
            (posControl.flags.estAglStatus == EST_TRUSTED && posControl.actualState.agl.pos.z <= navConfig()->general.land_slowdown_minalt)) {
 
             // Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
-            rcCommand[THROTTLE] = getThrottleIdleValue();
             ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
 
             // Stabilize ROLL axis on 0 degrees banking regardless of loiter radius and position
@@ -761,7 +760,7 @@ bool isFixedWingLandingDetected(void)
  *-----------------------------------------------------------*/
 void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
 {
-    rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
+    rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true);
 
     if (posControl.flags.estAltStatus >= EST_USABLE) {
         // target min descent rate 10m above takeoff altitude

src/main/navigation/navigation_fw_launch.c

@@ -234,19 +234,12 @@ static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTi
 
 /* Wing control Helpers */
 
-static bool isThrottleIdleEnabled(void)
-{
-    return currentBatteryProfile->nav.fw.launch_idle_throttle > getThrottleIdleValue();
-}
-
 static void applyThrottleIdleLogic(bool forceMixerIdle)
 {
-    if (isThrottleIdleEnabled() && !forceMixerIdle) {
+    if (forceMixerIdle) {
-        rcCommand[THROTTLE] = currentBatteryProfile->nav.fw.launch_idle_throttle;
+        ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);   // If MOTOR_STOP is enabled mixer will keep motor stopped, otherwise motor will run at idle
-    }
+    } else {
-    else {
+        rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->nav.fw.launch_idle_throttle, true);
-        ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);           // If MOTOR_STOP is enabled mixer will keep motor stopped
-        rcCommand[THROTTLE] = getThrottleIdleValue();   // If MOTOR_STOP is disabled, motors will spin given throttle value
     }
 }
 
@@ -290,7 +283,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs
     UNUSED(currentTimeUs);
 
     if (!throttleStickIsLow()) {
-        if (isThrottleIdleEnabled()) {
+        if (currentBatteryProfile->nav.fw.launch_idle_throttle > getThrottleIdleValue()) {
             return FW_LAUNCH_EVENT_SUCCESS;
         }
         else {
@@ -404,7 +397,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_
 
     const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
     const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time;
-    const uint16_t launchThrottle = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle);
+    const uint16_t launchThrottle = setDesiredThrottle(currentBatteryProfile->nav.fw.launch_throttle, false);
 
     if (elapsedTimeMs > motorSpinUpMs) {
         rcCommand[THROTTLE] = launchThrottle;
@@ -438,7 +431,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t
         }
     } else {
         initialTime = navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time;
-        rcCommand[THROTTLE] = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle);
+        rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->nav.fw.launch_throttle, false);
     }
 
     if (isLaunchMaxAltitudeReached()) {
@@ -468,7 +461,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr
         // Make a smooth transition from the launch state to the current state for pitch angle
         // Do the same for throttle when manual launch throttle isn't used
         if (!navConfig()->fw.launch_manual_throttle) {
-            const uint16_t launchThrottle = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle);
+            const uint16_t launchThrottle = setDesiredThrottle(currentBatteryProfile->nav.fw.launch_throttle, false);
             rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, launchThrottle, rcCommand[THROTTLE]);
         }
         fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]);

src/main/navigation/navigation_multicopter.c

@@ -273,7 +273,7 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
     }
 
     // Update throttle controller
-    rcCommand[THROTTLE] = posControl.rcAdjustment[THROTTLE];
+    rcCommand[THROTTLE] = setDesiredThrottle(posControl.rcAdjustment[THROTTLE], false);
 
     // Save processed throttle for future use
     rcCommandAdjustedThrottle = rcCommand[THROTTLE];
@@ -914,13 +914,14 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
     rcCommand[YAW] = 0;
     rcCommand[ROLL] = 0;
     rcCommand[PITCH] = 0;
-    rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
 
     /* Altitude sensors gone haywire, attempt to land regardless */
     if (posControl.flags.estAltStatus < EST_USABLE) {
         if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_DROP_IT) {
             rcCommand[THROTTLE] = getThrottleIdleValue();
+            return;
         }
+        rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true);
         return;
     }
 
@@ -946,7 +947,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
     }
 
     // Update throttle
-    rcCommand[THROTTLE] = posControl.rcAdjustment[THROTTLE];
+    rcCommand[THROTTLE] = setDesiredThrottle(posControl.rcAdjustment[THROTTLE], false);
 
     // Hold position if possible
     if ((posControl.flags.estPosStatus >= EST_USABLE)) {

src/main/navigation/navigation_rover_boat.c

@@ -138,7 +138,7 @@ void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags,
         rcCommand[ROLL] = 0;
         rcCommand[PITCH] = 0;
         rcCommand[YAW] = 0;
-        rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
+        rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true);
     } else if (navStateFlags & NAV_CTL_POS) {
         applyRoverBoatPositionController(currentTimeUs);
         applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs);