Merge branch 'master' into sh_mixer_profile

.gitignore

@@ -33,3 +33,5 @@ README.pdf
 # local changes only
 make/local.mk
 launch.json
+.vscode/tasks.json
+.vscode/c_cpp_properties.json

docs/Programming Framework.md

@@ -158,7 +158,7 @@ The flight mode operands return `true` when the mode is active. These are modes
 | 7             | HORIZON           | `true` when you are in the **Horizon** flight mode. |
 | 8             | AIR               | `true` when you the **Airmode** flight mode modifier is active. |
 | 9             | USER1             | `true` when the **USER 1** mode is active. |
-| 10            | USER2             | `true` when the **USER 21** mode is active. |
+| 10            | USER2             | `true` when the **USER 2** mode is active. |
 | 11            | COURSE_HOLD       | `true` when you are in the **Course Hold** flight mode. |
 | 12            | USER3             | `true` when the **USER 3** mode is active. |
 | 13            | USER4             | `true` when the **USER 4** mode is active. |

docs/Settings.md

@@ -4762,16 +4762,6 @@ Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO`, 'DJIWTF'
 
 ---
 
-### output_mode
-
-Output function assignment mode. AUTO assigns outputs according to the default mapping, SERVOS assigns all outputs to servos, MOTORS assigns all outputs to motors
-
-| Default | Min | Max |
-| --- | --- | --- |
-| AUTO |  |  |
-
----
-
 ### pid_iterm_limit_percent
 
 Limits max/min I-term value in stabilization PID controller. It solves the problem of servo saturation before take-off/throwing the airplane into the air. Or multirotors with low authority. By default, error accumulated in I-term can not exceed 33% of total pid throw (around 165us on deafult pidsum_limit of pitch/roll). Set 0 to disable completely.
@@ -4878,7 +4868,7 @@ Selection of pitot hardware.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| NONE |  |  |
+| VIRTUAL |  |  |
 
 ---
 

src/main/drivers/pwm_mapping.c

@@ -211,23 +211,7 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw)
 }
 
 static void timerHardwareOverride(timerHardware_t * timer) {
-    if (currentMixerConfig.outputMode == OUTPUT_MODE_SERVOS) {
+    
-        
-        //Motors are rewritten as servos
-        if (timer->usageFlags & (TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR)) {
-            timer->usageFlags &= ~(TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR);
-            timer->usageFlags |= TIM_USE_MC_SERVO | TIM_USE_FW_SERVO;
-        }
-        
-    } else if (currentMixerConfig.outputMode == OUTPUT_MODE_MOTORS) {
-        
-        // Servos are rewritten as motors
-        if (timer->usageFlags & (TIM_USE_MC_SERVO | TIM_USE_FW_SERVO)) {
-            timer->usageFlags &= ~(TIM_USE_MC_SERVO | TIM_USE_FW_SERVO);
-            timer->usageFlags |= TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR;
-        }
-    }
-
     switch (timerOverrides(timer2id(timer->tim))->outputMode) {
         case OUTPUT_MODE_MOTORS:
             if (timer->usageFlags & (TIM_USE_MC_SERVO | TIM_USE_FW_SERVO)) {

src/main/fc/fc_core.c

@@ -834,13 +834,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);
+        }
     }
 }
 
@@ -892,26 +904,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/settings.yaml

@@ -112,9 +112,6 @@ tables:
     enum: smartportFuelUnit_e
   - name: platform_type
     values: ["MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT"]
-  - name: output_mode
-    values: ["AUTO", "MOTORS", "SERVOS"]
-    enum: outputMode_e
   - name: tz_automatic_dst
     values: ["OFF", "EU", "USA"]
     enum: tz_automatic_dst_e
@@ -588,7 +585,7 @@ groups:
     members:
       - name: pitot_hardware
         description: "Selection of pitot hardware."
-        default_value: "NONE"
+        default_value: "VIRTUAL"
         table: pitot_hardware
       - name: pitot_lpf_milli_hz
         min: 0
@@ -1164,11 +1161,6 @@ groups:
         field: mixer_config.appliedMixerPreset
         min: -1
         max: INT16_MAX
-      - name: output_mode
-        description: "Output function assignment mode. AUTO assigns outputs according to the default mapping, SERVOS assigns all outputs to servos, MOTORS assigns all outputs to motors"
-        default_value: "AUTO"
-        field: mixer_config.outputMode
-        table: output_mode
       - name: motorstop_on_low
         description: "If enabled, motor will stop when throttle is low on this mixer_profile"
         default_value: OFF

src/main/flight/failsafe.c

@@ -146,6 +146,10 @@ static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
  */
 void failsafeReset(void)
 {
+    if (failsafeState.active) {  // can't reset if still active
+        return;
+    }
+
     failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND;
     failsafeState.rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY + failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND;
     failsafeState.validRxDataReceivedAt = 0;
@@ -157,6 +161,7 @@ void failsafeReset(void)
     failsafeState.phase = FAILSAFE_IDLE;
     failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
     failsafeState.activeProcedure = failsafeConfig()->failsafe_procedure;
+    failsafeState.controlling = false;
 
     failsafeState.lastGoodRcCommand[ROLL] = 0;
     failsafeState.lastGoodRcCommand[PITCH] = 0;
@@ -211,14 +216,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

@@ -83,7 +83,7 @@ PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig,
     .neutral = SETTING_3D_NEUTRAL_DEFAULT
 );
 
-PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 9);
+PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 10);
 
 PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
     .motorPwmProtocol = SETTING_MOTOR_PWM_PROTOCOL_DEFAULT,
@@ -230,6 +230,7 @@ void mixerInit(void)
 
 void mixerResetDisarmedMotors(void)
 {
+    getThrottleIdleValue();
 
     if (feature(FEATURE_REVERSIBLE_MOTORS)) {
         motorZeroCommand = reversibleMotorsConfig()->neutral;
@@ -237,7 +238,7 @@ void mixerResetDisarmedMotors(void)
         throttleRangeMax = motorConfig()->maxthrottle;
     } else {
         motorZeroCommand = motorConfig()->mincommand;
-        throttleRangeMin = getThrottleIdleValue();
+        throttleRangeMin = throttleIdleValue;
         throttleRangeMax = motorConfig()->maxthrottle;
     }
 
@@ -246,7 +247,7 @@ void mixerResetDisarmedMotors(void)
     if (ifMotorstopFeatureEnabled()) {
         motorValueWhenStopped = motorZeroCommand;
     } else {
-        motorValueWhenStopped = getThrottleIdleValue();
+        motorValueWhenStopped = throttleIdleValue;
     }
 
     // set disarmed motor values
@@ -491,6 +492,19 @@ void FAST_CODE mixTable(void)
         return;
     }
 #endif
+#ifdef USE_DEV_TOOLS
+    bool isDisarmed = !ARMING_FLAG(ARMED) || systemConfig()->groundTestMode;
+#else
+    bool isDisarmed = !ARMING_FLAG(ARMED);
+#endif
+    bool motorStopIsActive = getMotorStatus() != MOTOR_RUNNING && !isDisarmed;
+    if (isDisarmed || motorStopIsActive) {
+        for (int i = 0; i < motorCount; i++) {
+            motor[i] = isDisarmed ? motor_disarmed[i] : motorValueWhenStopped;
+        }
+        mixerThrottleCommand = motor[0];
+        return;
+    }
 
     int16_t input[3];   // RPY, range [-500:+500]
     // Allow direct stick input to motors in passthrough mode on airplanes
@@ -573,11 +587,11 @@ void FAST_CODE mixTable(void)
         throttleRangeMax = motorConfig()->maxthrottle;
 
         // Throttle scaling to limit max throttle when battery is full
-    #ifdef USE_PROGRAMMING_FRAMEWORK
+#ifdef USE_PROGRAMMING_FRAMEWORK
         mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * getThrottleScale(currentBatteryProfile->motor.throttleScale)) + throttleRangeMin;
-    #else
+#else
         mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * currentBatteryProfile->motor.throttleScale) + throttleRangeMin;
-    #endif
+#endif
         // Throttle compensation based on battery voltage
         if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) {
             mixerThrottleCommand = MIN(throttleRangeMin + (mixerThrottleCommand - throttleRangeMin) * calculateThrottleCompensationFactor(), throttleRangeMax);
@@ -586,7 +600,6 @@ void FAST_CODE mixTable(void)
 
     throttleMin = throttleRangeMin;
     throttleMax = throttleRangeMax;
-
     throttleRange = throttleMax - throttleMin;
 
     #define THROTTLE_CLIPPING_FACTOR    0.33f
@@ -606,64 +619,53 @@ void FAST_CODE mixTable(void)
 
     // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
     // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
-    if (ARMING_FLAG(ARMED)) {
+    for (int i = 0; i < motorCount; i++) {
-        const motorStatus_e currentMotorStatus = getMotorStatus();
+        motor[i] = rpyMix[i] + constrain(mixerThrottleCommand * currentMixer[i].throttle, throttleMin, throttleMax);
-        for (int i = 0; i < motorCount; i++) {
-            motor[i] = rpyMix[i] + constrain(mixerThrottleCommand * currentMixer[i].throttle, throttleMin, throttleMax);
 
-            if (failsafeIsActive()) {
+        if (failsafeIsActive()) {
-                motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle);
+            motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle);
-            } else {
+        } else {
-                motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax);
+            motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax);
-            }
-            
-            //stop motors
-            if (currentMixer[i].throttle <= 0.0f) {
-                motor[i] = motorZeroCommand;
-            }
-            //spin stopped motors only in mixer transition mode
-            if (isMixerTransitionMixing && currentMixer[i].throttle <= -1.05f && currentMixer[i].throttle >= -2.0f && (!feature(FEATURE_REVERSIBLE_MOTORS))) {
-                motor[i] = -currentMixer[i].throttle * 1000;
-                motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax);
-            }
-
-            // Motor stop handling
-            if (currentMotorStatus != MOTOR_RUNNING) {
-                motor[i] = motorValueWhenStopped;
-            }
-#ifdef USE_DEV_TOOLS
-            if (systemConfig()->groundTestMode) {
-                motor[i] = motorZeroCommand;
-            }
-#endif
         }
-    } else {
+        
-        for (int i = 0; i < motorCount; i++) {
+        //stop motors
-            motor[i] = motor_disarmed[i];
+        if (currentMixer[i].throttle <= 0.0f) {
+            motor[i] = motorZeroCommand;
+        }
+        //spin stopped motors only in mixer transition mode
+        if (isMixerTransitionMixing && currentMixer[i].throttle <= -1.05f && currentMixer[i].throttle >= -2.0f && (!feature(FEATURE_REVERSIBLE_MOTORS))) {
+            motor[i] = -currentMixer[i].throttle * 1000;
+            motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax);
         }
     }
 }
 
 int16_t getThrottlePercent(bool useScaled)
 {
-    int16_t thr = constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
+    int16_t thr = constrain(mixerThrottleCommand, PWM_RANGE_MIN, PWM_RANGE_MAX);
-    const int idleThrottle = getThrottleIdleValue();
+
-    
     if (useScaled) {
-       thr = (thr - idleThrottle) * 100 / (motorConfig()->maxthrottle - idleThrottle);
+       thr = (thr - throttleIdleValue) * 100 / (motorConfig()->maxthrottle - throttleIdleValue);
     } else {
         thr = (rxGetChannelValue(THROTTLE) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN);
     }
     return thr;
 }
 
+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);
+}
+
 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/flight/mixer.h

@@ -110,6 +110,7 @@ extern int mixerThrottleCommand;
 bool ifMotorstopFeatureEnabled(void);
 int getThrottleIdleValue(void);
 int16_t getThrottlePercent(bool);
+uint16_t setDesiredThrottle(uint16_t throttle, bool allowMotorStop);
 uint8_t getMotorCount(void);
 float getMotorMixRange(void);
 bool mixerIsOutputSaturated(void);

src/main/flight/mixer_profile.c

@@ -48,7 +48,6 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance)
                          .platformType = SETTING_PLATFORM_TYPE_DEFAULT,
                          .hasFlaps = SETTING_HAS_FLAPS_DEFAULT,
                          .appliedMixerPreset = SETTING_MODEL_PREVIEW_TYPE_DEFAULT, // This flag is not available in CLI and used by Configurator only
-                         .outputMode = SETTING_OUTPUT_MODE_DEFAULT,
                          .motorstopOnLow = SETTING_MOTORSTOP_ON_LOW_DEFAULT,
                          .PIDProfileLinking = SETTING_MIXER_PID_PROFILE_LINKING_DEFAULT,
                          .automated_switch = SETTING_MIXER_AUTOMATED_SWITCH_DEFAULT,

src/main/io/osd.c

@@ -831,13 +831,14 @@ static const char * osdArmingDisabledReasonMessage(void)
             // See handling of FAILSAFE_RX_LOSS_MONITORING in failsafe.c
             if (failsafePhase() == FAILSAFE_RX_LOSS_MONITORING) {
                 if (failsafeIsReceivingRxData()) {
-                    // If we're not using sticks, it means the ARM switch
+                    // reminder to disarm to exit FAILSAFE_RX_LOSS_MONITORING once timeout period ends
-                    // hasn't been off since entering FAILSAFE_RX_LOSS_MONITORING
+                    if (IS_RC_MODE_ACTIVE(BOXARM)) {
-                    // yet
+                        return OSD_MESSAGE_STR(OSD_MSG_TURN_ARM_SW_OFF);
-                    return OSD_MESSAGE_STR(OSD_MSG_TURN_ARM_SW_OFF);
+                    }
+                } else {
+                    // Not receiving RX data
+                    return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST);
                 }
-                // Not receiving RX data
-                return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST);
             }
             return OSD_MESSAGE_STR(OSD_MSG_DISABLED_BY_FS);
         case ARMING_DISABLED_NOT_LEVEL:
@@ -991,10 +992,10 @@ static const char * osdFailsafeInfoMessage(void)
 #if defined(USE_SAFE_HOME)
 static const char * divertingToSafehomeMessage(void)
 {
-	if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && posControl.safehomeState.isApplied) {
+    if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && posControl.safehomeState.isApplied) {
-	    return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME);
+        return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME);
-	}
+    }
-	return NULL;
+    return NULL;
 }
 #endif
 
@@ -1116,11 +1117,7 @@ bool osdUsingScaledThrottle(void)
  **/
 static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes_t *elemAttr)
 {
-    if (useScaled) {
+    buff[0] = SYM_BLANK;
-        buff[0] = SYM_SCALE;
-    } else {
-        buff[0] = SYM_BLANK;
-    }
     buff[1] = SYM_THR;
     if (navigationIsControllingThrottle()) {
         buff[0] = SYM_AUTO_THR0;
@@ -1135,7 +1132,14 @@ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes
         TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr);
     }
 #endif
-    tfp_sprintf(buff + 2, "%3d", getThrottlePercent(useScaled));
+    int8_t throttlePercent = getThrottlePercent(useScaled);
+    if ((useScaled && throttlePercent <= 0) || !ARMING_FLAG(ARMED)) {
+        const char* message = ARMING_FLAG(ARMED) ? throttlePercent == 0 ? "IDLE" : "STOP" : "DARM";
+        buff[0] = SYM_THR;
+        strcpy(buff + 1, message);
+        return;
+    }
+    tfp_sprintf(buff + 2, "%3d", throttlePercent);
 }
 
 /**
@@ -4445,14 +4449,14 @@ static void osdShowArmed(void)
             if (posControl.safehomeState.distance) { // safehome found during arming
                 if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) {
                     strcpy(buf, "SAFEHOME FOUND; MODE OFF");
-				} else {
+                } else {
-					char buf2[12]; // format the distance first
+                    char buf2[12]; // format the distance first
-					osdFormatDistanceStr(buf2, posControl.safehomeState.distance);
+                    osdFormatDistanceStr(buf2, posControl.safehomeState.distance);
-					tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, posControl.safehomeState.index);
+                    tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, posControl.safehomeState.index);
-				}
+                }
-				textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT;
+                textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT;
-				// write this message above the ARMED message to make it obvious
+                // write this message above the ARMED message to make it obvious
-				displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr);
+                displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr);
             }
 #endif
         } else {

src/main/navigation/navigation.c

@@ -1874,8 +1874,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINIS
 {
     UNUSED(previousState);
 
-    disarm(DISARM_NAVIGATION);
-
     return NAV_FSM_EVENT_NONE;
 }
 
@@ -2972,7 +2970,7 @@ void updateLandingStatus(timeMs_t currentTimeMs)
         }
     } else if (STATE(LANDING_DETECTED)) {
         pidResetErrorAccumulators();
-        if (navConfig()->general.flags.disarm_on_landing) {
+        if (navConfig()->general.flags.disarm_on_landing && !FLIGHT_MODE(FAILSAFE_MODE)) {
             ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
             disarm(DISARM_LANDING);
         } else if (!navigationInAutomaticThrottleMode()) {

src/main/navigation/navigation_fixedwing.c

@@ -651,7 +651,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
@@ -666,7 +666,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
@@ -766,7 +765,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

@@ -121,7 +121,7 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
     int16_t rcThrottleCorrection = pt1FilterApply4(&altholdThrottleFilterState, velocity_controller, NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
     rcThrottleCorrection = constrain(rcThrottleCorrection, thrCorrectionMin, thrCorrectionMax);
 
-    posControl.rcAdjustment[THROTTLE] = constrain(currentBatteryProfile->nav.mc.hover_throttle + rcThrottleCorrection, getThrottleIdleValue(), motorConfig()->maxthrottle);
+    posControl.rcAdjustment[THROTTLE] = setDesiredThrottle(currentBatteryProfile->nav.mc.hover_throttle + rcThrottleCorrection, false);
 }
 
 bool adjustMulticopterAltitudeFromRCInput(void)
@@ -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;
     }
 

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

src/main/target/AOCODARCF4V2/target.h

@@ -55,6 +55,7 @@
 
 #define USE_BARO_BMP280
 #define USE_BARO_MS5611
+#define USE_BARO_DPS310
 
 //*********** Magnetometer / Compass *************
 #define USE_MAG