diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index b79e97c396..720ca97fa3 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -212,10 +212,10 @@ static void updateArmingStatus(void) /* CHECK: Throttle */ if (!armingConfig()->fixed_wing_auto_arm) { // Don't want this check if fixed_wing_auto_arm is in use - machine arms on throttle > LOW - if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) { - ENABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE); - } else { + if (throttleStickIsLow()) { DISABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE); + } else { + ENABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE); } } @@ -630,13 +630,13 @@ void processRx(timeUs_t currentTimeUs) failsafeUpdateState(); - const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC); + const bool lowThrottle = throttleStickIsLow(); // When armed and motors aren't spinning, do beeps periodically if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY)) { static bool armedBeeperOn = false; - if (throttleStatus == THROTTLE_LOW) { + if (lowThrottle) { beeper(BEEPER_ARMED); armedBeeperOn = true; } else if (armedBeeperOn) { @@ -645,7 +645,7 @@ void processRx(timeUs_t currentTimeUs) } } - processRcStickPositions(throttleStatus); + processRcStickPositions(); processAirmode(); updateActivatedModes(); @@ -759,7 +759,7 @@ void processRx(timeUs_t currentTimeUs) pidResetErrorAccumulators(); } else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER) { - if (throttleStatus == THROTTLE_LOW) { + if (lowThrottle) { if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) { if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY))) { ENABLE_STATE(ANTI_WINDUP); @@ -778,7 +778,7 @@ void processRx(timeUs_t currentTimeUs) } } else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER_ONCE) { - if (throttleStatus == THROTTLE_LOW) { + if (lowThrottle) { if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) { if ((rollPitchStatus == CENTERED) && !STATE(ANTI_WINDUP_DEACTIVATED)) { ENABLE_STATE(ANTI_WINDUP); @@ -802,7 +802,7 @@ void processRx(timeUs_t currentTimeUs) else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) { DISABLE_STATE(ANTI_WINDUP); //This case applies only to MR when Airmode management is throttle threshold activated - if (throttleStatus == THROTTLE_LOW && !STATE(AIRMODE_ACTIVE)) { + if (lowThrottle && !STATE(AIRMODE_ACTIVE)) { pidResetErrorAccumulators(); } } diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 8ccc9c63de..807999c3f3 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -127,6 +127,11 @@ throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e return THROTTLE_HIGH; } +bool throttleStickIsLow(void) +{ + return calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW; +} + int16_t throttleStickMixedValue(void) { int16_t throttleValue; @@ -181,7 +186,7 @@ static void updateRcStickPositions(void) rcStickPositions = tmp; } -void processRcStickPositions(throttleStatus_e throttleStatus) +void processRcStickPositions(void) { static timeMs_t lastTickTimeMs = 0; static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors @@ -209,9 +214,11 @@ void processRcStickPositions(throttleStatus_e throttleStatus) bool armingSwitchIsActive = IS_RC_MODE_ACTIVE(BOXARM); emergencyArmingUpdate(armingSwitchIsActive); + const bool lowThrottle = throttleStickIsLow(); + if (STATE(AIRPLANE) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) { // Auto arm on throttle when using fixedwing and motorstop - if (throttleStatus != THROTTLE_LOW) { + if (!lowThrottle) { tryArm(); return; } @@ -227,7 +234,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus) if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && !failsafeIsActive()) { const timeMs_t disarmDelay = currentTimeMs - rcDisarmTimeMs; if (disarmDelay > armingConfig()->switchDisarmDelayMs) { - if (armingConfig()->disarm_kill_switch || (throttleStatus == THROTTLE_LOW)) { + if (armingConfig()->disarm_kill_switch || lowThrottle) { disarm(DISARM_SWITCH); } } diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 2637283e3d..65c5edeb11 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -112,6 +112,7 @@ bool isRollPitchStickDeflected(uint8_t deadband); throttleStatus_e calculateThrottleStatus(throttleStatusType_e type); int16_t throttleStickMixedValue(void); rollPitchStatus_e calculateRollPitchCenterStatus(void); -void processRcStickPositions(throttleStatus_e throttleStatus); +void processRcStickPositions(void); +bool throttleStickIsLow(void); int32_t getRcStickDeflection(int32_t axis); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 585a9ee079..097e5c3a99 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -387,7 +387,7 @@ void failsafeUpdateState(void) case FAILSAFE_IDLE: if (armed) { // Track throttle command below minimum time - if (THROTTLE_HIGH == calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC)) { + if (!throttleStickIsLow()) { failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; } if (!receivingRxDataAndNotFailsafeMode) { diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index ee4cf4ef54..c06fc5bb46 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -626,11 +626,8 @@ motorStatus_e getMotorStatus(void) } const bool fixedWingOrAirmodeNotActive = STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE); - const bool throttleStickLow = - (calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW); - - if (throttleStickLow && fixedWingOrAirmodeNotActive) { + if (throttleStickIsLow() && fixedWingOrAirmodeNotActive) { if ((navConfig()->general.flags.nav_overrides_motor_stop == NOMS_OFF_ALWAYS) && failsafeIsActive()) { // If we are in failsafe and user was holding stick low before it was triggered and nav_overrides_motor_stop is set to OFF_ALWAYS // and either on a plane or on a quad with inactive airmode - stop motor @@ -652,7 +649,6 @@ motorStatus_e getMotorStatus(void) return MOTOR_STOPPED_USER; } } - } return MOTOR_RUNNING; diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index ecde02c97c..59d8347ade 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4327,8 +4327,7 @@ bool isNavLaunchEnabled(void) bool abortLaunchAllowed(void) { // allow NAV_LAUNCH_MODE to be aborted if throttle is low or throttle stick position is < launch idle throttle setting - throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC); - return throttleStatus == THROTTLE_LOW || throttleStickMixedValue() < currentBatteryProfile->nav.fw.launch_idle_throttle; + return throttleStickIsLow() || throttleStickMixedValue() < currentBatteryProfile->nav.fw.launch_idle_throttle; } int32_t navigationGetHomeHeading(void) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 85d8fb4ea0..1c45f55f58 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -685,12 +685,11 @@ bool isFixedWingLandingDetected(void) { DEBUG_SET(DEBUG_LANDING, 4, 0); static bool fixAxisCheck = false; - const bool throttleIsLow = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW; // Basic condition to start looking for landing bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG)) || FLIGHT_MODE(FAILSAFE_MODE) - || (!navigationIsControllingThrottle() && throttleIsLow); + || (!navigationIsControllingThrottle() && throttleStickIsLow()); if (!startCondition || posControl.flags.resetLandingDetector) { return fixAxisCheck = posControl.flags.resetLandingDetector = false; diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 1515f46004..134e649dfa 100644 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -248,11 +248,6 @@ static void applyThrottleIdleLogic(bool forceMixerIdle) } } -static inline bool isThrottleLow(void) -{ - return calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW; -} - static inline bool isLaunchMaxAltitudeReached(void) { return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude); @@ -272,7 +267,7 @@ static inline bool isProbablyNotFlying(void) static void resetPidsIfNeeded(void) { // Don't use PID I-term and reset TPA filter until motors are started or until flight is detected - if (isProbablyNotFlying() || fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP || (navConfig()->fw.launch_manual_throttle && isThrottleLow())) { + if (isProbablyNotFlying() || fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP || (navConfig()->fw.launch_manual_throttle && throttleStickIsLow())) { pidResetErrorAccumulators(); pidResetTPAFilter(); } @@ -292,7 +287,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs { UNUSED(currentTimeUs); - if (!isThrottleLow()) { + if (!throttleStickIsLow()) { if (isThrottleIdleEnabled()) { return FW_LAUNCH_EVENT_SUCCESS; } @@ -312,7 +307,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(timeUs_t currentTimeUs) { - if (isThrottleLow()) { + if (throttleStickIsLow()) { return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE } @@ -330,7 +325,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(tim static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs) { - if (isThrottleLow()) { + if (throttleStickIsLow()) { return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE } @@ -349,7 +344,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs) { - if (isThrottleLow()) { + if (throttleStickIsLow()) { return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE } @@ -427,7 +422,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t if (navConfig()->fw.launch_manual_throttle) { // reset timers when throttle is low or until flight detected and abort launch regardless of launch settings - if (isThrottleLow()) { + if (throttleStickIsLow()) { fwLaunch.currentStateTimeUs = currentTimeUs; fwLaunch.pitchAngle = 0; if (isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband)) { diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index b9a77a4df2..acf65db1d3 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -176,14 +176,14 @@ bool adjustMulticopterAltitudeFromRCInput(void) void setupMulticopterAltitudeController(void) { - const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC); + const bool lowThrottle = throttleStickIsLow(); if (navConfig()->general.flags.use_thr_mid_for_althold) { altHoldThrottleRCZero = rcLookupThrottleMid(); } else { - // If throttle status is THROTTLE_LOW - use Thr Mid anyway - if (throttleStatus == THROTTLE_LOW) { + // If throttle is LOW - use Thr Mid anyway + if (lowThrottle) { altHoldThrottleRCZero = rcLookupThrottleMid(); } else { @@ -198,7 +198,7 @@ void setupMulticopterAltitudeController(void) // Force AH controller to initialize althold integral for pending takeoff on reset // Signal for that is low throttle _and_ low actual altitude - if (throttleStatus == THROTTLE_LOW && fabsf(navGetCurrentActualPositionAndVelocity()->pos.z) <= 50.0f) { + if (lowThrottle && fabsf(navGetCurrentActualPositionAndVelocity()->pos.z) <= 50.0f) { prepareForTakeoffOnReset = true; } } @@ -724,13 +724,12 @@ bool isMulticopterLandingDetected(void) { DEBUG_SET(DEBUG_LANDING, 4, 0); static timeUs_t landingDetectorStartedAt; - const bool throttleIsLow = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW; /* Basic condition to start looking for landing * Prevent landing detection if WP mission allowed during Failsafe (except landing states) */ bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG)) || (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE)) - || (!navigationIsFlyingAutonomousMode() && throttleIsLow); + || (!navigationIsFlyingAutonomousMode() && throttleStickIsLow()); if (!startCondition || posControl.flags.resetLandingDetector) { landingDetectorStartedAt = 0; diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index dfaa768bed..9652e15c4f 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -547,7 +547,6 @@ void currentMeterUpdate(timeUs_t timeDelta) case CURRENT_SENSOR_VIRTUAL: amperage = batteryMetersConfig()->current.offset; if (ARMING_FLAG(ARMED)) { - throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC); navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags(); bool allNav = navConfig()->general.flags.nav_overrides_motor_stop == NOMS_ALL_NAV && posControl.navState != NAV_STATE_IDLE; bool autoNav = navConfig()->general.flags.nav_overrides_motor_stop == NOMS_AUTO_ONLY && (stateFlags & (NAV_AUTO_RTH | NAV_AUTO_WP)); @@ -556,7 +555,7 @@ void currentMeterUpdate(timeUs_t timeDelta) if (allNav || autoNav) { // account for motors running in Nav modes with throttle low + motor stop throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; } else { - throttleOffset = ((throttleStatus == THROTTLE_LOW) && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000; + throttleOffset = (throttleStickIsLow() && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000; } int32_t throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); amperage += throttleFactor * batteryMetersConfig()->current.scale / 1000; diff --git a/src/main/telemetry/frsky_d.c b/src/main/telemetry/frsky_d.c index 0bc65d3457..7eb8a76003 100644 --- a/src/main/telemetry/frsky_d.c +++ b/src/main/telemetry/frsky_d.c @@ -195,10 +195,10 @@ static void sendThrottleOrBatterySizeAsRpm(void) { sendDataHead(ID_RPM); if (ARMING_FLAG(ARMED)) { - const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC); uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER; - if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) - throttleForRPM = 0; + if (throttleStickIsLow() && feature(FEATURE_MOTOR_STOP)) { + throttleForRPM = 0; + } serialize16(throttleForRPM); } else { serialize16((currentBatteryProfile->capacity.value / BLADE_NUMBER_DIVIDER));