From 9782fb15554ccc85d7435113770bbe345b569101 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 19 Jun 2016 15:21:26 +0200 Subject: [PATCH 1/2] Apply PID reduction to only Iterm. (RC1 behaviour) --- src/main/flight/mixer.c | 2 +- src/main/flight/pid.c | 14 ++++++++------ 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 6564964c9..d58f06294 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -825,7 +825,7 @@ void mixTable(void) rollPitchYawMix[i] = qMultiply(mixReduction,rollPitchYawMix[i]); } // Get the maximum correction by setting offset to center - throttleMin = throttleMax = throttleMin + (throttleRange / 2); + if (!escAndServoConfig->escDesyncProtection) throttleMin = throttleMax = throttleMin + (throttleRange / 2); } else { throttleMin = throttleMin + (rollPitchYawMixRange / 2); throttleMax = throttleMax - (rollPitchYawMixRange / 2); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index c41705c94..5b77c8c0b 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -174,13 +174,13 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, uint16_t max_angle_incli // 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 = (angleRate[axis] - gyroRate) * errorLimiter; + RateError = angleRate[axis] - gyroRate; // Smoothed Error for Derivative if (flightModeFlags && axis != YAW) { RateErrorSmooth = RateError; } else { - RateErrorSmooth = (angleRateSmooth[axis] - gyroRate) * errorLimiter; + RateErrorSmooth = angleRateSmooth[axis] - gyroRate; } // -----calculate P component @@ -194,7 +194,7 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, uint16_t max_angle_incli // -----calculate I component. uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, (int32_t)angleRate[axis]) : pidProfile->I8[axis]; - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * RateError * getdT() * kI, -250.0f, 250.0f); + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * errorLimiter * RateError * getdT() * kI, -250.0f, 250.0f); // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings @@ -299,13 +299,13 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, uint16_t max_angl // multiplication of rcCommand corresponds to changing the sticks scaling here gyroRate = gyroADC[axis] / 4; - RateError = (AngleRateTmp - gyroRate) * errorLimiter; + RateError = AngleRateTmp - gyroRate; // Smoothed Error for Derivative if (flightModeFlags && axis != YAW) { RateErrorSmooth = RateError; } else { - RateErrorSmooth = (AngleRateTmpSmooth - gyroRate) * errorLimiter; + RateErrorSmooth = AngleRateTmpSmooth - gyroRate; } // -----calculate P component @@ -323,7 +323,9 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, uint16_t max_angl // is normalized to cycle time = 2048. uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, AngleRateTmp) : pidProfile->I8[axis]; - errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * kI; + int32_t rateErrorLimited = errorLimiter * RateError; + + errorGyroI[axis] = errorGyroI[axis] + ((rateErrorLimited * (uint16_t)targetPidLooptime) >> 11) * kI; // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings From 0c5ea0f5c63455ce1bf91f4f8fcf70813768d796 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 19 Jun 2016 21:21:31 +0200 Subject: [PATCH 2/2] Fix Configurator Save Error --- src/main/io/serial_msp.c | 42 ++++++++++++++++++++-------------------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 285de3c3a..b1eb17c89 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1666,27 +1666,6 @@ static bool processInCommand(void) case MSP_SET_BF_CONFIG: - case MSP_SET_PID_ADVANCED_CONFIG : - masterConfig.gyro_sync_denom = read8(); - masterConfig.pid_process_denom = read8(); - masterConfig.use_unsyncedPwm = read8(); - masterConfig.fast_pwm_protocol = read8(); - masterConfig.motor_pwm_rate = read16(); - break; - case MSP_SET_FILTER_CONFIG : - masterConfig.gyro_soft_lpf_hz = read8(); - currentProfile->pidProfile.dterm_lpf_hz = read16(); - currentProfile->pidProfile.yaw_lpf_hz = read16(); - break; - case MSP_SET_ADVANCED_TUNING: - currentProfile->pidProfile.rollPitchItermIgnoreRate = read16(); - currentProfile->pidProfile.yawItermIgnoreRate = read16(); - currentProfile->pidProfile.yaw_p_limit = read16(); - break; - case MSP_SET_TEMPORARY_COMMANDS: - currentControlRateProfile->rcYawRate8 = read8(); - break; - #ifdef USE_QUAD_MIXER_ONLY read8(); // mixerMode ignored #else @@ -1801,6 +1780,27 @@ static bool processInCommand(void) // proceed as usual with MSP commands break; #endif + + case MSP_SET_PID_ADVANCED_CONFIG : + masterConfig.gyro_sync_denom = read8(); + masterConfig.pid_process_denom = read8(); + masterConfig.use_unsyncedPwm = read8(); + masterConfig.fast_pwm_protocol = read8(); + masterConfig.motor_pwm_rate = read16(); + break; + case MSP_SET_FILTER_CONFIG : + masterConfig.gyro_soft_lpf_hz = read8(); + currentProfile->pidProfile.dterm_lpf_hz = read16(); + currentProfile->pidProfile.yaw_lpf_hz = read16(); + break; + case MSP_SET_ADVANCED_TUNING: + currentProfile->pidProfile.rollPitchItermIgnoreRate = read16(); + currentProfile->pidProfile.yawItermIgnoreRate = read16(); + currentProfile->pidProfile.yaw_p_limit = read16(); + break; + case MSP_SET_TEMPORARY_COMMANDS: + currentControlRateProfile->rcYawRate8 = read8(); + break; default: // we do not know how to handle the (valid) message, indicate error MSP $M! return false;