From 74fdca926feafba72633ce89af548c15c602b5dd Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 23 Apr 2019 15:57:07 +0200 Subject: [PATCH 1/5] on MR allow airmode to be sticky and activated by throttle threshold --- src/main/fc/fc_core.c | 10 ++++++++-- src/main/fc/rc_controls.c | 5 +++-- src/main/fc/rc_controls.h | 8 ++++++++ src/main/fc/rc_modes.c | 41 ++++++++++++++++++++++++++++++++++++++- src/main/fc/rc_modes.h | 1 + src/main/fc/settings.yaml | 5 +++++ 6 files changed, 65 insertions(+), 5 deletions(-) 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 2137e488f9e..27b3e2ab8b2 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -120,6 +120,8 @@ tables: - name: iterm_relax_type values: ["GYRO", "SETPOINT"] enum: itermRelaxType_e + - name: airmodeHandlingType + values: ["STICK_CENTER", "THROTTLE_THRESHOLD"] groups: - name: PG_GYRO_CONFIG @@ -851,6 +853,9 @@ groups: field: deadband3d_throttle min: 0 max: 200 + - name: mc_airmode_type + field: airmodeHandlingType + table: airmodeHandlingType - name: PG_PID_PROFILE type: pidProfile_t From cd78c753b82b7c4eb403fb2e88d2ca5a196b3ba8 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 23 Apr 2019 19:06:18 +0200 Subject: [PATCH 2/5] fix bug --- src/main/fc/fc_core.c | 2 +- src/main/fc/rc_controls.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 7044182d63c..4155fb22599 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -610,7 +610,7 @@ void processRx(timeUs_t currentTimeUs) else { DISABLE_STATE(ANTI_WINDUP); } - } else if (rcControlsConfig()->airmodeHandlingType == AIRMODE_THROTTLE_THRESHOLD) { + } 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 && !isAirmodeActive()) { diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index cbe16b4ddec..4679334c437 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -22,7 +22,7 @@ #define AUTO_DISARM_DELAY_MIN 0 #define AUTO_DISARM_DELAY_MAX 60 -#define AIRMODE_THROTTLE_THRESHOLD 1250 +#define AIRMODE_THROTTLE_THRESHOLD 1300 typedef enum rc_alias { ROLL = 0, From 22777d9622fff42d7257345e93ebea977b0b3569 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 23 Apr 2019 19:30:09 +0200 Subject: [PATCH 3/5] make airmode activation threshold configurable --- src/main/fc/rc_controls.c | 3 ++- src/main/fc/rc_controls.h | 1 + src/main/fc/rc_modes.c | 2 +- src/main/fc/settings.yaml | 4 ++++ 4 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 20a2d2af2d6..0ceb63cff27 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -77,7 +77,8 @@ PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, .pos_hold_deadband = 20, .alt_hold_deadband = 50, .deadband3d_throttle = 50, - .airmodeHandlingType = STICK_CENTER + .airmodeHandlingType = STICK_CENTER, + .airmodeThrottleThreshold = AIRMODE_THROTTLE_THRESHOLD, ); 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 4679334c437..35579f90f26 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -85,6 +85,7 @@ typedef struct rcControlsConfig_s { 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 + uint16_t airmodeThrottleThreshold; // Throttle threshold for airmode initial activation } rcControlsConfig_t; PG_DECLARE(rcControlsConfig_t, rcControlsConfig); diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index ae44f13ce33..d6d8d78a1fd 100755 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -74,7 +74,7 @@ void processAirmode(void) { airmodeActivationFlag = false; } else if ( !airmodeActivationFlag && - rcCommand[THROTTLE] > AIRMODE_THROTTLE_THRESHOLD && + rcCommand[THROTTLE] > rcControlsConfig()->airmodeThrottleThreshold && (feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE)) ) { /* diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 27b3e2ab8b2..6013c8f28a7 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -856,6 +856,10 @@ groups: - name: mc_airmode_type field: airmodeHandlingType table: airmodeHandlingType + - name: mc_airmode_threshold + field: airmodeThrottleThreshold + min: 1000 + max: 2000 - name: PG_PID_PROFILE type: pidProfile_t From 10d39a197b6305103944be74ed525a2589e4b595 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 28 Apr 2019 20:30:13 +0200 Subject: [PATCH 4/5] Keep arimore activation flag as state --- src/main/fc/fc_core.c | 4 ++-- src/main/fc/rc_modes.c | 25 +++++++++++-------------- src/main/fc/rc_modes.h | 1 - src/main/fc/runtime_config.h | 1 + src/main/flight/mixer.c | 2 +- src/main/io/osd.c | 2 +- src/main/telemetry/crsf.c | 2 +- 7 files changed, 17 insertions(+), 20 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 4494224252a..7fd31075df0 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -552,7 +552,7 @@ void processRx(timeUs_t currentTimeUs) } else if (STATE(FIXED_WING) || rcControlsConfig()->airmodeHandlingType == STICK_CENTER) { if (throttleStatus == THROTTLE_LOW) { - if (isAirmodeActive() && !failsafeIsActive() && ARMING_FLAG(ARMED)) { + if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) { rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(); // ANTI_WINDUP at centred stick with MOTOR_STOP is needed on MRs and not needed on FWs @@ -574,7 +574,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 && !isAirmodeActive()) { + if (throttleStatus == THROTTLE_LOW && !STATE(AIRMODE_ACTIVE)) { pidResetErrorAccumulators(); } } diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index 3264ca18e8e..7392bf648a7 100755 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -40,8 +40,6 @@ static uint8_t specifiedConditionCountPerMode[CHECKBOX_ITEM_COUNT]; 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 @@ -58,16 +56,20 @@ PG_REGISTER(modeActivationOperatorConfig_t, modeActivationOperatorConfig, PG_MOD void processAirmode(void) { if (STATE(FIXED_WING) || rcControlsConfig()->airmodeHandlingType == STICK_CENTER) { - airmodeActivationFlag = feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE); + if (feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE)) { + ENABLE_STATE(AIRMODE_ACTIVE); + } else { + DISABLE_STATE(AIRMODE_ACTIVE); + } } else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) { if (!ARMING_FLAG(ARMED)) { /* * Disarm disables airmode immediately */ - airmodeActivationFlag = false; + DISABLE_STATE(AIRMODE_ACTIVE); } else if ( - !airmodeActivationFlag && + !STATE(AIRMODE_ACTIVE) && rcCommand[THROTTLE] > rcControlsConfig()->airmodeThrottleThreshold && (feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE)) ) { @@ -75,28 +77,23 @@ void processAirmode(void) { * Airmode is allowed to be active only after ARMED and then THROTTLE goes above * activation threshold */ - airmodeActivationFlag = true; + ENABLE_STATE(AIRMODE_ACTIVE); } else if ( - airmodeActivationFlag && + STATE(AIRMODE_ACTIVE) && !feature(FEATURE_AIRMODE) && !IS_RC_MODE_ACTIVE(BOXAIRMODE) ) { /* * When user disables BOXAIRMODE, turn airmode off as well */ - airmodeActivationFlag = false; + DISABLE_STATE(AIRMODE_ACTIVE); } } else { - airmodeActivationFlag = false; + DISABLE_STATE(AIRMODE_ACTIVE); } } -bool isAirmodeActive(void) -{ - return airmodeActivationFlag; -} - #if defined(USE_NAV) bool isUsingNavigationModes(void) { diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index af89f437052..43f5946384b 100755 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -121,7 +121,6 @@ void rcModeUpdate(boxBitmask_t *newState); bool isModeActivationConditionPresent(boxId_e modeId); void processAirmode(void); -bool isAirmodeActive(void); bool isUsingNavigationModes(void); bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range); diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 26eab3847d9..ae727ee80d1 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -104,6 +104,7 @@ typedef enum { NAV_CRUISE_BRAKING_BOOST = (1 << 12), NAV_CRUISE_BRAKING_LOCKED = (1 << 13), NAV_EXTRA_ARMING_SAFETY_BYPASSED = (1 << 14), // nav_extra_arming_safey was bypassed. Keep it until power cycle. + AIRMODE_ACTIVE = (1 << 15), } stateFlags_t; #define DISABLE_STATE(mask) (stateFlags &= ~(mask)) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index f2c1351a070..364121697ec 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -404,7 +404,7 @@ motorStatus_e getMotorStatus(void) } if (rxGetChannelValue(THROTTLE) < rxConfig()->mincheck) { - if ((STATE(FIXED_WING) || !isAirmodeActive()) && (!(navigationIsFlyingAutonomousMode() && navConfig()->general.flags.auto_overrides_motor_stop)) && (!failsafeIsActive())) { + if ((STATE(FIXED_WING) || !STATE(AIRMODE_ACTIVE)) && (!(navigationIsFlyingAutonomousMode() && navConfig()->general.flags.auto_overrides_motor_stop)) && (!failsafeIsActive())) { return MOTOR_STOPPED_USER; } } diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 805b3ad37d6..148703e1281 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1597,7 +1597,7 @@ static bool osdDrawSingleElement(uint8_t item) p = "ANGL"; else if (FLIGHT_MODE(HORIZON_MODE)) p = "HOR "; - else if (isAirmodeActive()) + else if (STATE(AIRMODE_ACTIVE)) p = "AIR "; displayWrite(osdDisplayPort, elemPosX, elemPosY, p); diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index f4188e19a7f..2dcda52627e 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -296,7 +296,7 @@ void crsfFrameFlightMode(sbuf_t *dst) // use same logic as OSD, so telemetry displays same flight text as OSD when armed const char *flightMode = "OK"; if (ARMING_FLAG(ARMED)) { - if (isAirmodeActive()) { + if (STATE(AIRMODE_ACTIVE)) { flightMode = "AIR"; } else { flightMode = "ACRO"; From 05aaeab6ae4ae0359ef2545d8dfd1ad054436c65 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 28 Apr 2019 20:57:43 +0200 Subject: [PATCH 5/5] Review changes --- src/main/fc/rc_modes.c | 22 ++++++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index 7392bf648a7..01f84af7f41 100755 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -54,8 +54,16 @@ boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0); PG_REGISTER(modeActivationOperatorConfig_t, modeActivationOperatorConfig, PG_MODE_ACTIVATION_OPERATOR_CONFIG, 0); -void processAirmode(void) { - if (STATE(FIXED_WING) || rcControlsConfig()->airmodeHandlingType == STICK_CENTER) { +static void processAirmodeAirplane(void) { + if (feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE)) { + ENABLE_STATE(AIRMODE_ACTIVE); + } else { + DISABLE_STATE(AIRMODE_ACTIVE); + } +} + +static void processAirmodeMultirotor(void) { + if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER) { if (feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE)) { ENABLE_STATE(AIRMODE_ACTIVE); } else { @@ -94,6 +102,16 @@ void processAirmode(void) { } } +void processAirmode(void) { + + if (STATE(FIXED_WING)) { + processAirmodeAirplane(); + } else { + processAirmodeMultirotor(); + } + +} + #if defined(USE_NAV) bool isUsingNavigationModes(void) {