diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 97359ea6d38..7044182d63c 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -485,7 +485,7 @@ void processRx(timeUs_t currentTimeUs) } processRcStickPositions(throttleStatus); - + processAirmode(); updateActivatedModes(); #ifdef USE_PINIOBOX @@ -589,7 +589,7 @@ void processRx(timeUs_t currentTimeUs) /* In MANUAL mode we reset integrators prevent I-term wind-up (PID output is not used in MANUAL) */ pidResetErrorAccumulators(); } - else { + else if (STATE(FIXED_WING) || rcControlsConfig()->airmodeHandlingType == STICK_CENTER) { if (throttleStatus == THROTTLE_LOW) { if (isAirmodeActive() && !failsafeIsActive() && ARMING_FLAG(ARMED)) { rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(); @@ -610,6 +610,12 @@ void processRx(timeUs_t currentTimeUs) else { DISABLE_STATE(ANTI_WINDUP); } + } else if (rcControlsConfig()->airmodeHandlingType == AIRMODE_THROTTLE_THRESHOLD) { + DISABLE_STATE(ANTI_WINDUP); + //This case applies only to MR when Airmode management is throttle threshold activated + if (throttleStatus == THROTTLE_LOW && !isAirmodeActive()) { + pidResetErrorAccumulators(); + } } if (mixerConfig()->platformType == PLATFORM_AIRPLANE) { diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 64b33504b7b..20a2d2af2d6 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -69,14 +69,15 @@ stickPositions_e rcStickPositions; FASTRAM int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW -PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 1); PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, .deadband = 5, .yaw_deadband = 5, .pos_hold_deadband = 20, .alt_hold_deadband = 50, - .deadband3d_throttle = 50 + .deadband3d_throttle = 50, + .airmodeHandlingType = STICK_CENTER ); PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 1); diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 51d49884e02..cbe16b4ddec 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -22,6 +22,8 @@ #define AUTO_DISARM_DELAY_MIN 0 #define AUTO_DISARM_DELAY_MAX 60 +#define AIRMODE_THROTTLE_THRESHOLD 1250 + typedef enum rc_alias { ROLL = 0, PITCH, @@ -51,6 +53,11 @@ typedef enum { CENTERED } rollPitchStatus_e; +typedef enum { + STICK_CENTER = 0, + THROTTLE_THRESHOLD +} airmodeAndAntiWindupHandlingType_e; + typedef enum { ROL_LO = (1 << (2 * ROLL)), ROL_CE = (3 << (2 * ROLL)), @@ -77,6 +84,7 @@ typedef struct rcControlsConfig_s { uint8_t pos_hold_deadband; // Adds ability to adjust the Hold-position when moving the sticks (assisted mode) uint8_t alt_hold_deadband; // Defines the neutral zone of throttle stick during altitude hold uint16_t deadband3d_throttle; // default throttle deadband from MIDRC + uint8_t airmodeHandlingType; // Defaults to ANTI_WINDUP triggered at sticks centered } rcControlsConfig_t; PG_DECLARE(rcControlsConfig_t, rcControlsConfig); diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index de02ddfc2c7..ae44f13ce33 100755 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -31,6 +31,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" +#include "fc/runtime_config.h" #include "rx/rx.h" @@ -40,6 +41,8 @@ static bool isUsingSticksToArm = true; static bool isUsingNAVModes = false; #endif +static EXTENDED_FASTRAM uint8_t airmodeActivationFlag = false; + boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e // TODO(alberto): It looks like we can now safely remove this assert, since everything @@ -59,9 +62,45 @@ bool isUsingSticksForArming(void) return isUsingSticksToArm; } +void processAirmode(void) { + if (STATE(FIXED_WING) || rcControlsConfig()->airmodeHandlingType == STICK_CENTER) { + airmodeActivationFlag = feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE); + } else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) { + + if (!ARMING_FLAG(ARMED)) { + /* + * Disarm disables airmode immediately + */ + airmodeActivationFlag = false; + } else if ( + !airmodeActivationFlag && + rcCommand[THROTTLE] > AIRMODE_THROTTLE_THRESHOLD && + (feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE)) + ) { + /* + * Airmode is allowed to be active only after ARMED and then THROTTLE goes above + * activation threshold + */ + airmodeActivationFlag = true; + } else if ( + airmodeActivationFlag && + !feature(FEATURE_AIRMODE) && + !IS_RC_MODE_ACTIVE(BOXAIRMODE) + ) { + /* + * When user disables BOXAIRMODE, turn airmode off as well + */ + airmodeActivationFlag = false; + } + + } else { + airmodeActivationFlag = false; + } +} + bool isAirmodeActive(void) { - return feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE); + return airmodeActivationFlag; } #if defined(USE_NAV) diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 1a90da24577..803d474f83f 100755 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -121,6 +121,7 @@ void rcModeUpdate(boxBitmask_t *newState); bool isModeActivationConditionPresent(boxId_e modeId); bool isUsingSticksForArming(void); +void processAirmode(void); bool isAirmodeActive(void); bool isUsingNavigationModes(void); bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 51fc6f95187..032a08a6983 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -114,6 +114,8 @@ tables: values: ["PT1", "BIQUAD"] - name: log_level values: ["ERROR", "WARNING", "INFO", "VERBOSE", "DEBUG"] + - name: airmodeHandlingType + values: ["STICK_CENTER", "THROTTLE_THRESHOLD"] groups: - name: PG_GYRO_CONFIG @@ -845,6 +847,9 @@ groups: field: deadband3d_throttle min: 0 max: 200 + - name: mc_airmode_type + field: airmodeHandlingType + table: airmodeHandlingType - name: PG_PID_PROFILE type: pidProfile_t