diff --git a/src/main/config/config.c b/src/main/config/config.c index 910954cdc..f7d1cb57a 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -398,7 +398,7 @@ static void resetConf(void) #endif featureSet(FEATURE_FAILSAFE); - featureSet(FEATURE_ONESHOT125); + featureSet(FEATURE_SUPEREXPO); // global settings masterConfig.current_profile_index = 0; // default profile @@ -461,8 +461,8 @@ static void resetConf(void) masterConfig.rxConfig.max_aux_channel = 6; masterConfig.rxConfig.superExpoFactor = 30; masterConfig.rxConfig.airModeActivateThreshold = 1350; - masterConfig.rxConfig.superExpoFactorYaw = 30; - masterConfig.rxConfig.superExpoYawMode = 0; + masterConfig.rxConfig.superExpoFactorYaw = 40; + masterConfig.rxConfig.superExpoYawMode = 1; resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges); diff --git a/src/main/config/config.h b/src/main/config/config.h index 55ae18185..24fe20ea6 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -44,6 +44,8 @@ typedef enum { FEATURE_BLACKBOX = 1 << 19, FEATURE_CHANNEL_FORWARDING = 1 << 20, FEATURE_TRANSPONDER = 1 << 21, + FEATURE_AIRMODE = 1 << 22, + FEATURE_SUPEREXPO = 1 << 23, } features_e; void handleOneshotFeatureChangeOnRestart(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3e741c0c0..783c627ec 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -209,7 +209,7 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_ uint16_t kP = (pidProfile->dynamic_pid) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis]; // -----calculate P component - if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) { + if ((isSuperExpoActive() && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) { PTerm = (luxPTermScale * kP * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig)); } else { PTerm = luxPTermScale * RateError * kP * tpaFactor; @@ -334,7 +334,7 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate uint16_t kP = (pidProfile->dynamic_pid) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis]; // -----calculate P component - if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) { + if ((isSuperExpoActive() && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) { PTerm = (kP * PIDweight[axis] / 100) * (AngleRateTmp - (int32_t)(gyroRate * calculateExpoPlus(axis, rxConfig))) >> 7; } else { PTerm = (RateError * kP * PIDweight[axis] / 100) >> 7; diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index fdde2cf1a..a72efd990 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -70,6 +70,13 @@ int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+ uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e +bool isAirmodeActive(void) { + return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE)); +} + +bool isSuperExpoActive(void) { + return (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) || feature(FEATURE_SUPEREXPO)); +} void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) { #ifndef BLACKBOX diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index eaec277ae..8cbb078a4 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -243,6 +243,8 @@ typedef struct adjustmentState_s { #define MAX_ADJUSTMENT_RANGE_COUNT 15 +bool isAirmodeActive(void); +bool isSuperExpoActive(void); void resetAdjustmentStates(void); void configureAdjustment(uint8_t index, uint8_t auxChannelIndex, const adjustmentConfig_t *adjustmentConfig); void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index a400d7509..ea6801e07 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -194,7 +194,8 @@ static const char * const featureNames[] = { "SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE", "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125", - "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", NULL + "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", "SUPEREXPO", + NULL }; // sync this with rxFailsafeChannelMode_e diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index f243b3001..a65b224b8 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -542,8 +542,8 @@ void mspInit(serialConfig_t *serialConfig) activeBoxIds[activeBoxIdCount++] = BOXHORIZON; } - activeBoxIds[activeBoxIdCount++] = BOXAIRMODE; - activeBoxIds[activeBoxIdCount++] = BOXSUPEREXPO; + if (!feature(FEATURE_AIRMODE)) activeBoxIds[activeBoxIdCount++] = BOXAIRMODE; + if (!feature(FEATURE_SUPEREXPO)) activeBoxIds[activeBoxIdCount++] = BOXSUPEREXPO; activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH; if (sensors(SENSOR_BARO)) { diff --git a/src/main/mw.c b/src/main/mw.c index 125674c9e..54401519b 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -454,7 +454,7 @@ void updateMagHold(void) void processRx(void) { static bool armedBeeperOn = false; - static bool airmodeIsActivated; + static bool wasAirmodeIsActivated; calculateRxChannelsAndUpdateFailsafe(currentTime); @@ -477,15 +477,15 @@ void processRx(void) throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); - if (IS_RC_MODE_ACTIVE(BOXAIRMODE) && ARMING_FLAG(ARMED)) { - if (rcCommand[THROTTLE] >= masterConfig.rxConfig.airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset + if (isAirmodeActive() && ARMING_FLAG(ARMED)) { + if (rcCommand[THROTTLE] >= masterConfig.rxConfig.airModeActivateThreshold) wasAirmodeIsActivated = true; // Prevent Iterm from being reset } else { - airmodeIsActivated = false; + wasAirmodeIsActivated = false; } /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */ - if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) { + if (throttleStatus == THROTTLE_LOW && !wasAirmodeIsActivated) { pidResetErrorGyroState(); }