1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 11:29:58 +03:00

Use wasThrottleRaised in place of isAirmodeActivated where appropriate (#13968)

* use wasThrottleRaised in place of isAirmodeActivated where airmode isn't really needed

* fix unit test

* fix typo

* remove unnecessary check

* final changes and clarificartions

* Update src/main/flight/mixer.c

Co-authored-by: Ivan Efimov <gendalf44@yandex.ru>

---------

Co-authored-by: Mark Haslinghuis <mark@numloq.nl>
Co-authored-by: Ivan Efimov <gendalf44@yandex.ru>
This commit is contained in:
ctzsnooze 2024-10-17 19:46:54 +11:00 committed by GitHub
parent 58fc8bbbb8
commit fc52b6b4ff
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
13 changed files with 42 additions and 37 deletions

View file

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

View file

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

View file

@ -72,7 +72,7 @@ void rcModeUpdate(const boxBitmask_t *newState)
rcModeActivationMask = *newState;
}
bool airmodeIsEnabled(void)
bool isAirmodeEnabled(void)
{
return airmodeEnabled;
}

View file

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

View file

@ -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

View file

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

View file

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

View file

@ -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";
}

View file

@ -1314,7 +1314,7 @@ extern "C" {
return false;
}
bool airmodeIsEnabled() {
bool isAirmodeEnabled() {
return false;
}

View file

@ -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

View file

@ -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; }

View file

@ -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;}

View file

@ -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)
{