diff --git a/src/main/config/config.c b/src/main/config/config.c index 58f626410f..c5034bad3a 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -331,6 +331,7 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) { void resetMixerConfig(mixerConfig_t *mixerConfig) { mixerConfig->yaw_motor_direction = 1; + mixerConfig->agressive_airmode = 0; mixerConfig->yaw_jump_prevention_limit = 200; #ifdef USE_SERVOS mixerConfig->tri_unarmed_servo = 1; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 8aa6a328ee..ae6eb57ed8 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -53,6 +53,12 @@ #include "config/runtime_config.h" #include "config/config.h" +typedef enum { + THROTTLE_3D_NEUTRAL = 0, + THROTTLE_3D_POSITIVE, + THROTTLE_3D_NEGATIVE +} throttle3dState_e; + uint8_t motorCount; int16_t motor[MAX_SUPPORTED_MOTORS]; @@ -67,8 +73,6 @@ static rxConfig_t *rxConfig; static mixerMode_e currentMixerMode; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; -bool motorLimitReached = false; - #ifdef USE_SERVOS static uint8_t servoRuleCount = 0; static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; @@ -778,27 +782,33 @@ void mixTable(void) // Scale roll/pitch/yaw uniformly to fit within throttle range int16_t rollPitchYawMixRange = rollPitchYawMixMax - rollPitchYawMixMin; int16_t throttleRange, throttle; - int16_t throttleMin, throttleMax = 0; - static bool flightDirection3dReversed; - + int16_t throttleMin, throttleMax; + throttle3dState_e throttle3dPosition = THROTTLE_3D_NEUTRAL; // Initiate in Neutral before all throttle checks throttle = rcCommand[THROTTLE]; // Find min and max throttle based on condition if (feature(FEATURE_3D)) { - static int16_t throttleMinPrevious, throttleMaxPrevious, throttlePrevious; + static int16_t throttleMinPrevious = 0, throttleMaxPrevious = 0, throttlePrevious = 0; + if (rcData[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)) { throttleMax = flight3DConfig->deadband3d_low; throttleMin = escAndServoConfig->minthrottle; - flightDirection3dReversed = true; + throttle3dPosition = THROTTLE_3D_NEGATIVE; } else if (rcData[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { throttleMax = escAndServoConfig->maxthrottle; throttleMin = flight3DConfig->deadband3d_high; - flightDirection3dReversed = false; + throttle3dPosition = THROTTLE_3D_POSITIVE; } else { - if (!throttleMin) { /* when starting in neutral */ + // when starting in neutral go positive and when coming from positive. Keep positive throttle within deadband + if ((throttle3dPosition == THROTTLE_3D_NEUTRAL && !throttleMinPrevious) + || (throttle3dPosition == THROTTLE_3D_NEUTRAL && (throttleMinPrevious >= flight3DConfig->deadband3d_high))) { throttleMax = escAndServoConfig->maxthrottle; throttle = throttleMin = flight3DConfig->deadband3d_high; - } else { + // When coming from negative. Keep negative throttle within deadband + } else if (throttle3dPosition == THROTTLE_3D_NEUTRAL && (throttleMinPrevious <= flight3DConfig->deadband3d_low)) { + throttleMax = escAndServoConfig->mincommand; + throttle = throttleMin = flight3DConfig->deadband3d_low; + } else { throttleMax = throttleMaxPrevious; throttleMin = throttleMinPrevious; throttle = throttlePrevious; @@ -819,6 +829,9 @@ void mixTable(void) motorLimitReached = true; for (i = 0; i < motorCount; i++) { rollPitchYawMix[i] = (rollPitchYawMix[i] * throttleRange) / rollPitchYawMixRange; + + // Get the max correction from center when agressivity enabled. (Some setups don't like this option) + if (mixerConfig->agressive_airmode) throttleMin = throttleMax = throttleMin + (throttleRange / 2); } } else { motorLimitReached = false; @@ -829,15 +842,12 @@ void mixTable(void) // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. for (i = 0; i < motorCount; i++) { - motor[i] = rollPitchYawMix[i] + constrainf(throttle * currentMixer[i].throttle, throttleMin, throttleMax); + motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax); - /* Double code. Preparations for full mixer replacement to airMode mixer. Copy from old mixer*/ - - // TODO - Should probably not be needed for constraining failsafe, but keep it till it is investigated. if (isFailsafeActive) { motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle); } else if (feature(FEATURE_3D)) { - if (flightDirection3dReversed) { + if (throttle3dPosition == THROTTLE_3D_NEGATIVE) { motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, flight3DConfig->deadband3d_low); } else { motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle); @@ -854,6 +864,7 @@ void mixTable(void) } } + // Disarmed mode if (!ARMING_FLAG(ARMED)) { for (i = 0; i < motorCount; i++) { motor[i] = motor_disarmed[i]; diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index da90b1f7fd..e8c4193703 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -71,6 +71,7 @@ typedef struct mixer_s { typedef struct mixerConfig_s { int8_t yaw_motor_direction; + uint8_t agressive_airmode; uint16_t yaw_jump_prevention_limit; // make limit configurable (original fixed value was 100) #ifdef USE_SERVOS uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed @@ -189,6 +190,7 @@ extern int16_t servo[MAX_SUPPORTED_SERVOS]; bool isMixerUsingServos(void); void writeServos(void); void filterServos(void); +bool motorLimitReached; #endif extern int16_t motor[MAX_SUPPORTED_MOTORS]; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 2c8fd1d304..1ee3b743c5 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -50,9 +50,6 @@ extern float dT; extern bool motorLimitReached; -bool lowThrottleWindupProtection; - -#define PREVENT_WINDUP(x,y) { if (ABS(x) > ABS(y)) { if (x < 0) { x = -ABS(y); } else { x = ABS(y); } } } int16_t axisPID[3]; @@ -201,11 +198,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f); if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) { - errorGyroIf[axis] *= scaleItermToRcInput(axis); - if (motorLimitReached || lowThrottleWindupProtection) { - PREVENT_WINDUP(errorGyroIf[axis], errorGyroIfLimit[axis]); + errorGyroIf[axis] = (int32_t) (errorGyroIf[axis] * scaleItermToRcInput(axis)); + if (antiWindupProtection || motorLimitReached) { + errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]); } else { - errorGyroIfLimit[axis] = errorGyroIf[axis]; + errorGyroIfLimit[axis] = ABS(errorGyroIf[axis]); } } @@ -349,10 +346,10 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) { errorGyroI[axis] = (int32_t) (errorGyroI[axis] * scaleItermToRcInput(axis)); - if (motorLimitReached || lowThrottleWindupProtection) { - PREVENT_WINDUP(errorGyroI[axis], errorGyroILimit[axis]); + if (antiWindupProtection || motorLimitReached) { + errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]); } else { - errorGyroILimit[axis] = errorGyroI[axis]; + errorGyroILimit[axis] = ABS(errorGyroI[axis]); } } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index cbfa9af3c9..42b4188922 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -78,6 +78,7 @@ typedef struct acroPlus_s { extern int16_t axisPID[XYZ_AXIS_COUNT]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; +bool antiWindupProtection; void pidSetController(pidControllerType_e type); void pidResetErrorGyro(void); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index db53f67724..d53faddc0b 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -617,6 +617,7 @@ const clivalue_t valueTable[] = { { "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.yaw_control_direction, .config.minmax = { -1, 1 } }, { "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_motor_direction, .config.minmax = { -1, 1 } }, + { "agressive_airmode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.agressive_airmode, .config.lookup = { TABLE_OFF_ON } }, { "yaw_jump_prevention_limit", VAR_UINT16 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_jump_prevention_limit, .config.minmax = { YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH } }, #ifdef USE_SERVOS { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } }, diff --git a/src/main/mw.c b/src/main/mw.c index 7e139c9747..c5c36bcb1d 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -117,7 +117,7 @@ static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the m extern uint32_t currentTime; extern uint8_t PIDweight[3]; -extern bool lowThrottleWindupProtection; +extern bool antiWindupProtection; static bool isRXDataNew; @@ -478,16 +478,22 @@ void processRx(void) } throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); - rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(&masterConfig.rxConfig); + rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(&masterConfig.rxConfig); + /* 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) { - if (IS_RC_MODE_ACTIVE(BOXAIRMODE) && rollPitchStatus == CENTERED) { /* Anti Windup when roll / pitch stick centered */ - lowThrottleWindupProtection = true; + if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { + if (rollPitchStatus == CENTERED) { + antiWindupProtection = true; + } else { + antiWindupProtection = false; + } } else { pidResetErrorGyro(); } } else { - lowThrottleWindupProtection = false; + antiWindupProtection = false; } // When armed and motors aren't spinning, do beeps and then disarm