diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index db298b700f..c6d5504c14 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -801,7 +801,7 @@ void mixTable(void) } // adjust feedback to scale PID error inputs to our limitations. - rpy_limiting = constrainf(((rpy_limiting * th_range) / rpy_range), 0.2f, 1.0f); + rpy_limiting = constrainf(((float)th_range / rpy_range), 0.1f, 1.0f); } if (ARMING_FLAG(ARMED)) { @@ -854,7 +854,7 @@ void mixTable(void) // If we're at minimum throttle and FEATURE_MOTOR_STOP enabled, // do not spin the motors. motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle); - if (((rcData[THROTTLE]) < rxConfig->mincheck) && !(IS_RC_MODE_ACTIVE(BOXAIRMODE))) { + if (((rcData[THROTTLE]) < rxConfig->mincheck)) { if (feature(FEATURE_MOTOR_STOP)) { motor[i] = escAndServoConfig->mincommand; } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 039a1f55ff..45e3619c45 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -161,10 +161,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa } } - if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { - AngleRate = AngleRate * rpy_limiting; - } - gyroRate = gyroADC[axis] * gyro.scale; // gyro output scaled to dps // --------low-level gyro-based PID. ---------- @@ -173,6 +169,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // multiplication of rcCommand corresponds to changing the sticks scaling here RateError = AngleRate - gyroRate; + if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { + RateError = RateError * rpy_limiting; + } + // -----calculate P component PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100; @@ -283,16 +283,16 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat } } - if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { - AngleRateTmp = AngleRateTmp * rpy_limiting; - } - // --------low-level gyro-based PID. ---------- // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes // -----calculate scaled error.AngleRates // multiplication of rcCommand corresponds to changing the sticks scaling here RateError = AngleRateTmp - (gyroADC[axis] / 4); + if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { + RateError = RateError * rpy_limiting; + } + // -----calculate P component PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 5e425f6a0c..345521aa27 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -126,7 +126,7 @@ typedef struct modeActivationCondition_s { #define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep) -#define SHOULD_RESET_ERRORS ((throttleStatus == THROTTLE_LOW && !(IS_RC_MODE_ACTIVE(BOXAIRMODE))) || !(ARMING_FLAG(ARMED)) || failsafeIsActive()) +#define SHOULD_RESET_ERRORS ((throttleStatus == THROTTLE_LOW && !(IS_RC_MODE_ACTIVE(BOXAIRMODE))) || !(ARMING_FLAG(ARMED)) || failsafeIsActive()|| feature(FEATURE_MOTOR_STOP)) typedef struct controlRateConfig_s { uint8_t rcRate8;