diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 28d0ae7df0..528ae7c188 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -202,7 +202,7 @@ bool canUseLaunchControl(void) if (!isFixedWing() && !isUsingSticksForArming() // require switch arming for safety && IS_RC_MODE_ACTIVE(BOXLAUNCHCONTROL) - && (!featureIsEnabled(FEATURE_MOTOR_STOP) || airmodeIsEnabled()) // can't use when motors are stopped + && (!featureIsEnabled(FEATURE_MOTOR_STOP) || isAirmodeEnabled()) // either not using motor_stop, or motor_stop is blocked by Airmode && !featureIsEnabled(FEATURE_3D) // pitch control is not 3D aware && (flightModeFlags == 0)) { // don't want to use unless in acro mode return true; @@ -769,14 +769,13 @@ uint8_t calculateThrottlePercentAbs(void) return abs(calculateThrottlePercent()); } -static bool airmodeIsActivated; +static bool throttleRaised = false; -bool isAirmodeActivated(void) +bool wasThrottleRaised(void) { - return airmodeIsActivated; + return throttleRaised; } - /* * processRx called from taskUpdateRxMain */ @@ -803,17 +802,23 @@ bool processRx(timeUs_t currentTimeUs) const bool throttleActive = calculateThrottleStatus() != THROTTLE_LOW; const uint8_t throttlePercent = calculateThrottlePercentAbs(); const bool launchControlActive = isLaunchControlActive(); + static bool isAirmodeActive; - if (airmodeIsEnabled() && ARMING_FLAG(ARMED) && !launchControlActive) { - // once throttle exceeds activate threshold, airmode latches active until disarm + if (ARMING_FLAG(ARMED)) { if (throttlePercent >= rxConfig()->airModeActivateThreshold) { - airmodeIsActivated = true; + throttleRaised = true; // Latch true until disarm + } + if (isAirmodeEnabled() && !launchControlActive) { + isAirmodeActive = throttleRaised; } } else { - airmodeIsActivated = false; + throttleRaised = false; + isAirmodeActive = false; } - if (ARMING_FLAG(ARMED) && (airmodeIsActivated || throttleActive || launchControlActive || isFixedWing())) { + // Note: If Airmode is enabled, on arming, iTerm and PIDs will be off until throttle exceeds the threshold (OFF while disarmed) + // If not, iTerm will be off at low throttle, with pidStabilisationState determining whether PIDs will be active + if (ARMING_FLAG(ARMED) && (isAirmodeActive || throttleActive || launchControlActive || isFixedWing())) { pidSetItermReset(false); pidStabilisationState(PID_STABILISATION_ON); } else { @@ -841,7 +846,7 @@ bool processRx(timeUs_t currentTimeUs) // - sticks are active and have deflection greater than runaway_takeoff_deactivate_stick_percent // - pidSum on all axis is less then runaway_takeoff_deactivate_pidlimit bool inStableFlight = false; - if (!featureIsEnabled(FEATURE_MOTOR_STOP) || airmodeIsEnabled() || throttleActive) { // are motors running? + if (!featureIsEnabled(FEATURE_MOTOR_STOP) || isAirmodeEnabled() || throttleActive) { // are motors running? const uint8_t lowThrottleLimit = pidConfig()->runaway_takeoff_deactivate_throttle; const uint8_t midThrottleLimit = constrain(lowThrottleLimit * 2, lowThrottleLimit * 2, RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT); if ((((throttlePercent >= lowThrottleLimit) && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT)) || (throttlePercent >= midThrottleLimit)) @@ -931,7 +936,7 @@ void processRxModes(timeUs_t currentTimeUs) && featureIsEnabled(FEATURE_MOTOR_STOP) && !isFixedWing() && !featureIsEnabled(FEATURE_3D) - && !airmodeIsEnabled() + && !isAirmodeEnabled() && !FLIGHT_MODE(GPS_RESCUE_MODE) // disable auto-disarm when GPS Rescue is active ) { if (isUsingSticksForArming()) { @@ -1025,8 +1030,8 @@ void processRxModes(timeUs_t currentTimeUs) && sensors(SENSOR_ACC) // and we have altitude data && isAltitudeAvailable() - // and we have already taken off (to prevent activation on the ground), then enable althold - && isAirmodeActivated()) { + // prevent activation until after takeoff + && wasThrottleRaised()) { if (!FLIGHT_MODE(ALT_HOLD_MODE)) { ENABLE_FLIGHT_MODE(ALT_HOLD_MODE); } @@ -1162,7 +1167,8 @@ static FAST_CODE_NOINLINE void subTaskPidController(timeUs_t currentTimeUs) && !crashFlipModeActive && !runawayTakeoffTemporarilyDisabled && !FLIGHT_MODE(GPS_RESCUE_MODE) // disable Runaway Takeoff triggering if GPS Rescue is active - && (!featureIsEnabled(FEATURE_MOTOR_STOP) || airmodeIsEnabled() || (calculateThrottleStatus() != THROTTLE_LOW))) { + // check that motors are running + && (!featureIsEnabled(FEATURE_MOTOR_STOP) || isAirmodeEnabled() || (calculateThrottleStatus() != THROTTLE_LOW))) { if (((fabsf(pidData[FD_PITCH].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD) || (fabsf(pidData[FD_ROLL].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD) diff --git a/src/main/fc/core.h b/src/main/fc/core.h index baa76c30bf..a7aefac35a 100644 --- a/src/main/fc/core.h +++ b/src/main/fc/core.h @@ -91,7 +91,7 @@ int8_t calculateThrottlePercent(void); uint8_t calculateThrottlePercentAbs(void); bool areSticksActive(uint8_t stickPercentLimit); void runawayTakeoffTemporaryDisable(uint8_t disableFlag); -bool isAirmodeActivated(); +bool wasThrottleRaised(); timeUs_t getLastDisarmTimeUs(void); bool isTryingToArm(); void resetTryingToArm(); diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index ce2bc54ac4..7fa904bd7a 100644 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -72,7 +72,7 @@ void rcModeUpdate(const boxBitmask_t *newState) rcModeActivationMask = *newState; } -bool airmodeIsEnabled(void) +bool isAirmodeEnabled(void) { return airmodeEnabled; } diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 0fe33c5b46..c13d31c62b 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -142,7 +142,7 @@ typedef struct modeActivationProfile_s { bool IS_RC_MODE_ACTIVE(boxId_e boxId); void rcModeUpdate(const boxBitmask_t *newState); -bool airmodeIsEnabled(void); +bool isAirmodeEnabled(void); bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range); void updateActivatedModes(void); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index d23e7c9b46..925aee1ff8 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -228,7 +228,7 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) currentThrottleInputRange = PWM_RANGE; #ifdef USE_DYN_IDLE if (mixerRuntime.dynIdleMinRps > 0.0f) { - const float maxIncrease = isAirmodeActivated() + const float maxIncrease = wasThrottleRaised() ? mixerRuntime.dynIdleMaxIncrease : mixerRuntime.dynIdleStartIncrease; float minRps = getMinMotorFrequencyHz(); DEBUG_SET(DEBUG_DYN_IDLE, 3, lrintf(minRps * 10.0f)); @@ -679,6 +679,9 @@ static void applyMixerAdjustment(float *motorMix, const float motorMixMin, const FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) { + const bool launchControlActive = isLaunchControlActive(); + const bool airmodeEnabled = isAirmodeEnabled() || launchControlActive; + // Find min and max throttle based on conditions. Throttle has to be known before mixing calculateThrottleAndCurrentMotorEndpoints(currentTimeUs); @@ -686,8 +689,6 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) return; // if crash flip mode has been applied to the motors, mixing is done } - const bool launchControlActive = isLaunchControlActive(); - motorMixer_t * activeMixer = &mixerRuntime.currentMixer[0]; #ifdef USE_LAUNCH_CONTROL if (launchControlActive && (currentPidProfile->launchControlMode == LAUNCH_CONTROL_MODE_PITCHONLY)) { @@ -781,8 +782,6 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) } // The following fixed throttle values will not be shown in the blackbox log - // ?? Should they be influenced by airmode? If not, should go after the apply airmode code. - const bool airmodeEnabled = airmodeIsEnabled() || launchControlActive; #ifdef USE_YAW_SPIN_RECOVERY // 50% throttle provides the maximum authority for yaw recovery when airmode is not active. // When airmode is active the throttle setting doesn't impact recovery authority. @@ -816,6 +815,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) motorMixRange = motorMixMax - motorMixMin; + // note that here airmodeEnabled is true also when Launch Control is active switch (mixerConfig()->mixer_type) { case MIXER_LEGACY: applyMixerAdjustment(motorMix, motorMixMin, motorMixMax, airmodeEnabled); @@ -835,10 +835,9 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) if (featureIsEnabled(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !mixerRuntime.feature3dEnabled - && !airmodeEnabled - && !FLIGHT_MODE(GPS_RESCUE_MODE | ALT_HOLD_MODE) // disable motor_stop while GPS Rescue / Altitude Hold is active + && !airmodeEnabled // not with airmode or launch control + && !FLIGHT_MODE(GPS_RESCUE_MODE | ALT_HOLD_MODE) // not in autopilot modes && (rcData[THROTTLE] < rxConfig()->mincheck)) { - // motor_stop handling applyMotorStop(); } else { // Apply the mix to motor endpoints diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index d7a8f79f0a..febcfa80fa 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -758,7 +758,7 @@ STATIC_UNIT_TESTED void applyAbsoluteControl(const int axis, const float gyroRat acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate; } - if (isAirmodeActivated()) { + if (wasThrottleRaised()) { axisError[axis] = constrainf(axisError[axis] + acErrorRate * pidRuntime.dT, -pidRuntime.acErrorLimit, pidRuntime.acErrorLimit); const float acCorrection = constrainf(axisError[axis] * pidRuntime.acGain, -pidRuntime.acLimit, pidRuntime.acLimit); @@ -840,8 +840,8 @@ float pidGetAirmodeThrottleOffset(void) static FAST_CODE_NOINLINE void disarmOnImpact(void) { - // if, after takeoff... - if (isAirmodeActivated() + // if, being armed, and after takeoff... + if (wasThrottleRaised() // and, either sticks are centred and throttle zeroed, && ((getMaxRcDeflectionAbs() < 0.05f && mixerGetRcThrottle() < 0.05f) // we could test here for stage 2 failsafe (including both landing or GPS Rescue) diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index 9cf07b4a3f..14cc510a2d 100644 --- a/src/main/osd/osd_elements.c +++ b/src/main/osd/osd_elements.c @@ -1066,7 +1066,7 @@ static void osdElementFlymode(osdElementParms_t *element) strcpy(element->buff, "HOR "); } else if (IS_RC_MODE_ACTIVE(BOXACROTRAINER)) { strcpy(element->buff, "ATRN"); - } else if (airmodeIsEnabled()) { + } else if (isAirmodeEnabled()) { strcpy(element->buff, "AIR "); } else { strcpy(element->buff, "ACRO"); diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 9f3dfb8228..16b32eaea6 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -405,7 +405,7 @@ void crsfFrameFlightMode(sbuf_t *dst) flightMode = "ALTH"; } else if (FLIGHT_MODE(HORIZON_MODE)) { flightMode = "HOR"; - } else if (airmodeIsEnabled()) { + } else if (isAirmodeEnabled()) { flightMode = "AIR"; } diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 40ebe0a074..0812c863cc 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -1314,7 +1314,7 @@ extern "C" { return false; } - bool airmodeIsEnabled() { + bool isAirmodeEnabled() { return false; } diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index 9bcc165666..658ac55534 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -24,7 +24,7 @@ #include "gtest/gtest.h" #include "build/debug.h" -bool simulatedAirmodeEnabled = true; +bool simulatedThrottleRaised = true; float simulatedSetpointRate[3] = { 0,0,0 }; float simulatedPrevSetpointRate[3] = { 0,0,0 }; float simulatedRcDeflection[3] = { 0,0,0 }; @@ -93,7 +93,7 @@ extern "C" { float getMotorMixRange(void) { return simulatedMotorMixRange; } float getSetpointRate(int axis) { return simulatedSetpointRate[axis]; } - bool isAirmodeActivated(void) { return simulatedAirmodeEnabled; } + bool wasThrottleRaised(void) { return simulatedThrottleRaised; } float getRcDeflectionAbs(int axis) { return fabsf(simulatedRcDeflection[axis]); } // used by auto-disarm code diff --git a/src/test/unit/rx_spi_expresslrs_telemetry_unittest.cc b/src/test/unit/rx_spi_expresslrs_telemetry_unittest.cc index 155515e828..1409b61897 100644 --- a/src/test/unit/rx_spi_expresslrs_telemetry_unittest.cc +++ b/src/test/unit/rx_spi_expresslrs_telemetry_unittest.cc @@ -420,7 +420,7 @@ extern "C" { bool telemetryIsSensorEnabled(sensor_e) {return true; } bool sensors(uint32_t ) { return true; } - bool airmodeIsEnabled(void) {return airMode; } + bool isAirmodeEnabled(void) {return airMode; } bool isBatteryVoltageConfigured(void) { return true; } bool isAmperageConfigured(void) { return true; } diff --git a/src/test/unit/telemetry_crsf_msp_unittest.cc b/src/test/unit/telemetry_crsf_msp_unittest.cc index 1ae7b5659c..d56241d370 100644 --- a/src/test/unit/telemetry_crsf_msp_unittest.cc +++ b/src/test/unit/telemetry_crsf_msp_unittest.cc @@ -292,7 +292,7 @@ extern "C" { bool featureIsEnabled(uint32_t) {return false;} - bool airmodeIsEnabled(void) {return true;} + bool isAirmodeEnabled(void) {return true;} mspDescriptor_t mspDescriptorAlloc(void) {return 0;} diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index 374071a5e8..b0ff149ca2 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -343,7 +343,7 @@ bool telemetryIsSensorEnabled(sensor_e) {return true;} portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e) {return PORTSHARING_NOT_SHARED;} -bool airmodeIsEnabled(void) {return airMode;} +bool isAirmodeEnabled(void) {return airMode;} int32_t getAmperage(void) {