1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 08:15:30 +03:00

Smoothed Derivative from Error // Fix Iterm accumulation issues // Defaultss

Saturation rework
This commit is contained in:
borisbstyle 2016-06-07 01:19:34 +02:00
parent 49defe8c68
commit d214f8602d
10 changed files with 104 additions and 103 deletions

View file

@ -49,8 +49,9 @@
#include "config/runtime_config.h"
extern uint8_t motorCount;
extern bool motorLimitReached;
uint32_t targetPidLooptime;
extern float errorLimiter;
extern float angleRate[3], angleRateSmooth[2];
int16_t axisPID[3];
@ -61,13 +62,13 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
uint8_t PIDweight[3];
static int32_t errorGyroI[3], errorGyroILimit[3];
static int32_t errorGyroI[3];
#ifndef SKIP_PID_LUXFLOAT
static float errorGyroIf[3], errorGyroIfLimit[3];
static float errorGyroIf[3];
#endif
static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig);
static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig);
pidControllerFuncPtr pid_controller = pidMultiWiiRewrite; // which pid controller are we using, defaultMultiWii
@ -75,38 +76,14 @@ void setTargetPidLooptime(uint8_t pidProcessDenom) {
targetPidLooptime = targetLooptime * pidProcessDenom;
}
float calculateRate(int axis, const controlRateConfig_t *controlRateConfig) {
float angleRate;
if (isSuperExpoActive()) {
float rcFactor = (axis == YAW) ? (ABS(rcCommand[axis]) / (500.0f * (controlRateConfig->rcYawRate8 / 100.0f))) : (ABS(rcCommand[axis]) / (500.0f * (controlRateConfig->rcRate8 / 100.0f)));
rcFactor = 1.0f / (constrainf(1.0f - (rcFactor * (controlRateConfig->rates[axis] / 100.0f)), 0.01f, 1.00f));
angleRate = rcFactor * ((27 * rcCommand[axis]) / 16.0f);
} else {
angleRate = (float)((controlRateConfig->rates[axis] + 27) * rcCommand[axis]) / 16.0f;
}
return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection
}
uint16_t getDynamicKp(int axis, const pidProfile_t *pidProfile) {
uint16_t dynamicKp;
uint32_t dynamicFactor = constrain(ABS(rcCommand[axis] << 8) / DYNAMIC_PTERM_STICK_THRESHOLD, 0, 1 << 7);
dynamicKp = ((pidProfile->P8[axis] << 8) + (pidProfile->P8[axis] * dynamicFactor)) >> 8;
return dynamicKp;
}
uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile) {
uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate) {
uint16_t dynamicKi;
uint16_t resetRate;
resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
uint32_t dynamicFactor = ((resetRate << 16) / (resetRate + ABS(gyroADC[axis]))) >> 8;
uint16_t dynamicFactor = (1 << 8) - constrain((ABS(angleRate) << 6) / resetRate, 0, 1 << 8);
dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8;
return dynamicKi;
@ -137,12 +114,12 @@ static filterStatePt1_t deltaFilterState[3];
static filterStatePt1_t yawFilterState;
#ifndef SKIP_PID_LUXFLOAT
static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
static void pidLuxFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{
float RateError, AngleRate, gyroRate;
float RateError, gyroRate, RateErrorSmooth;
float ITerm,PTerm,DTerm;
static float lastRate[3];
static float lastRateError[2];
float delta;
int axis;
float horizonLevelStrength = 1;
@ -171,40 +148,43 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
// ACRO mode, control is GYRO based, direct sticks control is applied to rate PID
AngleRate = calculateRate(axis, controlRateConfig);
// Yaw control is GYRO based, direct sticks control is applied to rate PID
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
// calculate error angle and limit the angle to the max inclination
#ifdef GPS
const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
const float errorAngle = (constrain(2 * rcCommandSmooth[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here
#else
const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
const float errorAngle = (constrain(2 * rcCommandSmooth[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here
#endif
if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode - control is angle based, so control loop is needed
AngleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f;
angleRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f;
} else {
// HORIZON mode - direct sticks control is applied to rate PID
// mix up angle error to desired AngleRate to add a little auto-level feel
AngleRate += errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f;
angleRate[axis] = angleRateSmooth[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f);
}
}
gyroRate = gyroADCf[axis] / 4.0f; // gyro output scaled to rewrite scale
gyroRate = gyroADCf[axis] / 4.0f; // gyro output scaled to old rewrite scale
// --------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 = AngleRate - gyroRate;
RateError = (angleRate[axis] - gyroRate) * errorLimiter;
uint16_t kP = (pidProfile->dynamic_pid) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis];
// Smoothed Error for Derivative
if (flightModeFlags && axis != YAW) {
RateErrorSmooth = RateError;
} else {
RateErrorSmooth = (angleRateSmooth[axis] - gyroRate) * errorLimiter;
}
// -----calculate P component
PTerm = luxPTermScale * RateError * kP * tpaFactor;
PTerm = luxPTermScale * RateError * pidProfile->P8[axis] * tpaFactor;
// Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) {
@ -212,16 +192,10 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
}
// -----calculate I component.
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile) : pidProfile->I8[axis];
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);
if (motorLimitReached) {
errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]);
} else {
errorGyroIfLimit[axis] = ABS(errorGyroIf[axis]);
}
// 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
ITerm = errorGyroIf[axis];
@ -241,8 +215,8 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
DTerm = 0.0f; // needed for blackbox
} else {
delta = -(gyroRate - lastRate[axis]);
lastRate[axis] = gyroRate;
delta = RateErrorSmooth - lastRateError[axis];
lastRateError[axis] = RateErrorSmooth;
// Divide delta by targetLooptime to get differential (ie dr/dt)
delta *= (1.0f / getdT());
@ -271,13 +245,13 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
}
#endif
static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{
int axis;
int32_t PTerm, ITerm, DTerm, delta;
static int32_t lastRate[3];
int32_t AngleRateTmp, RateError, gyroRate;
static int32_t lastRateError[3];
int32_t AngleRateTmp, AngleRateTmpSmooth, RateError, gyroRate, RateErrorSmooth;
int8_t horizonLevelStrength = 100;
@ -297,15 +271,16 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
for (axis = 0; axis < 3; axis++) {
// -----Get the desired angle rate depending on flight mode
AngleRateTmp = (int32_t)calculateRate(axis, controlRateConfig);
AngleRateTmp = (int32_t)angleRate[axis];
if (axis != YAW) AngleRateTmpSmooth = (int32_t)angleRateSmooth[axis];
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
// calculate error angle and limit the angle to max configured inclination
#ifdef GPS
const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
const int32_t errorAngle = constrain(2 * rcCommandSmooth[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#else
const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
const int32_t errorAngle = constrain(2 * rcCommandSmooth[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#endif
if (FLIGHT_MODE(ANGLE_MODE)) {
@ -314,7 +289,7 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
} else {
// HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel,
// horizonLevelStrength is scaled to the stick input
AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4;
AngleRateTmp = AngleRateTmpSmooth + ((errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4);
}
}
@ -323,12 +298,18 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
// -----calculate scaled error.AngleRates
// multiplication of rcCommand corresponds to changing the sticks scaling here
gyroRate = gyroADC[axis] / 4;
RateError = AngleRateTmp - gyroRate;
uint16_t kP = (pidProfile->dynamic_pid) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis];
RateError = (AngleRateTmp - gyroRate) * errorLimiter;
// Smoothed Error for Derivative
if (flightModeFlags && axis != YAW) {
RateErrorSmooth = RateError;
} else {
RateErrorSmooth = (AngleRateTmpSmooth - gyroRate) * errorLimiter;
}
// -----calculate P component
PTerm = (RateError * kP * PIDweight[axis] / 100) >> 7;
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
// Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) {
@ -340,7 +321,7 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
// Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
// Time correction (to avoid different I scaling for different builds based on average cycle time)
// is normalized to cycle time = 2048.
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile) : pidProfile->I8[axis];
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, AngleRateTmp) : pidProfile->I8[axis];
errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * kI;
@ -348,12 +329,6 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
// I coefficient (I8) moved before integration to make limiting independent from PID settings
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
if (motorLimitReached) {
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
} else {
errorGyroILimit[axis] = ABS(errorGyroI[axis]);
}
ITerm = errorGyroI[axis] >> 13;
//-----calculate D-term
@ -371,8 +346,8 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
DTerm = 0; // needed for blackbox
} else {
delta = -(gyroRate - lastRate[axis]);
lastRate[axis] = gyroRate;
delta = RateErrorSmooth - lastRateError[axis];
lastRateError[axis] = RateErrorSmooth;
// Divide delta by targetLooptime to get differential (ie dr/dt)
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5;