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:
parent
58fc8bbbb8
commit
fc52b6b4ff
13 changed files with 42 additions and 37 deletions
|
@ -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)
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -72,7 +72,7 @@ void rcModeUpdate(const boxBitmask_t *newState)
|
|||
rcModeActivationMask = *newState;
|
||||
}
|
||||
|
||||
bool airmodeIsEnabled(void)
|
||||
bool isAirmodeEnabled(void)
|
||||
{
|
||||
return airmodeEnabled;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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");
|
||||
|
|
|
@ -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";
|
||||
}
|
||||
|
||||
|
|
|
@ -1314,7 +1314,7 @@ extern "C" {
|
|||
return false;
|
||||
}
|
||||
|
||||
bool airmodeIsEnabled() {
|
||||
bool isAirmodeEnabled() {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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; }
|
||||
|
|
|
@ -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;}
|
||||
|
||||
|
|
|
@ -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)
|
||||
{
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue