1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 16:55:29 +03:00

revert changes of pid controller

This commit is contained in:
shota 2023-09-20 01:12:48 +09:00
parent 59e421fba2
commit 49e69f0aaa
7 changed files with 62 additions and 77 deletions

View file

@ -1222,6 +1222,16 @@ Iterm is not allowed to grow when stick position is above threshold. This solves
---
### fw_iterm_throw_limit
Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely.
| Default | Min | Max |
| --- | --- | --- |
| 165 | FW_ITERM_THROW_LIMIT_MIN | FW_ITERM_THROW_LIMIT_MAX |
---
### fw_level_pitch_gain
I-gain for the pitch trim for self-leveling flight modes. Higher values means that AUTOTRIM will be faster but might introduce oscillations
@ -1914,7 +1924,7 @@ Calculated 1G of Acc axis Z to use in INS
### iterm_windup
Used to prevent Iterm accumulation on during maneuvers. Iterm accumulation will be dampened when motors are reaching it's limit (when requested motor correction range is close percentage specified by this parameter, when value is set below 50 no accumulation occors when iterm_windup+50 of motor correction range is reached)
Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter)
| Default | Min | Max |
| --- | --- | --- |
@ -4762,16 +4772,6 @@ Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO`, 'DJIWTF'
---
### pid_iterm_limit_percent
Limits max/min I-term value in stabilization PID controller. It solves the problem of servo saturation before take-off/throwing the airplane into the air. Or multirotors with low authority. By default, error accumulated in I-term can not exceed 33% of total pid throw (around 165us on deafult pidsum_limit of pitch/roll). Set 0 to disable completely.
| Default | Min | Max |
| --- | --- | --- |
| 33 | 0 | 200 |
---
### pid_type
Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID`
@ -5672,19 +5672,9 @@ See tpa_rate.
---
### tpa_on_yaw
Throttle PID attenuation also reduces influence on YAW for multi-rotor, Should be set to ON for tilting rotors.
| Default | Min | Max |
| --- | --- | --- |
| OFF | OFF | ON |
---
### tpa_rate
Throttle PID attenuation reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate.
Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate.
| Default | Min | Max |
| --- | --- | --- |

View file

@ -91,7 +91,6 @@
)
#define MIN(a, b) _CHOOSE(<, a, b)
#define MAX(a, b) _CHOOSE(>, a, b)
#define SIGN(a) ((a >= 0) ? 1 : -1)
#define _ABS_II(x, var) \
( __extension__ ({ \

View file

@ -44,7 +44,6 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *instance)
.rcMid8 = SETTING_THR_MID_DEFAULT,
.rcExpo8 = SETTING_THR_EXPO_DEFAULT,
.dynPID = SETTING_TPA_RATE_DEFAULT,
.dynPID_on_YAW = SETTING_TPA_ON_YAW_DEFAULT,
.pa_breakpoint = SETTING_TPA_BREAKPOINT_DEFAULT,
.fixedWingTauMs = SETTING_FW_TPA_TIME_CONSTANT_DEFAULT
},

View file

@ -29,7 +29,6 @@ typedef struct controlRateConfig_s {
uint8_t rcMid8;
uint8_t rcExpo8;
uint8_t dynPID;
bool dynPID_on_YAW;
uint16_t pa_breakpoint; // Breakpoint where TPA is activated
uint16_t fixedWingTauMs; // Time constant of airplane TPA PT1-filter
} throttle;

View file

@ -1267,7 +1267,7 @@ groups:
min: 0
max: 100
- name: tpa_rate
description: "Throttle PID attenuation reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate."
description: "Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate."
default_value: 0
field: throttle.dynPID
min: 0
@ -1278,11 +1278,6 @@ groups:
field: throttle.pa_breakpoint
min: PWM_RANGE_MIN
max: PWM_RANGE_MAX
- name: tpa_on_yaw
description: "Throttle PID attenuation also reduces influence on YAW for multi-rotor, Should be set to ON for tilting rotors."
type: bool
field: throttle.dynPID_on_YAW
default_value: OFF
- name: fw_tpa_time_constant
description: "TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. See **PID Attenuation and scaling** Wiki for full details."
default_value: 1500
@ -1856,6 +1851,12 @@ groups:
default_value: 0
min: 0
max: 200
- name: fw_iterm_throw_limit
description: "Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely."
default_value: 165
field: fixedWingItermThrowLimit
min: FW_ITERM_THROW_LIMIT_MIN
max: FW_ITERM_THROW_LIMIT_MAX
- name: fw_reference_airspeed
description: "Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present."
default_value: 1500
@ -1899,17 +1900,11 @@ groups:
min: PID_SUM_LIMIT_MIN
max: PID_SUM_LIMIT_MAX
- name: iterm_windup
description: "Used to prevent Iterm accumulation on during maneuvers. Iterm accumulation will be dampened when motors are reaching it's limit (when requested motor correction range is close percentage specified by this parameter, when value is set below 50 no accumulation occors when iterm_windup+50 of motor correction range is reached)"
description: "Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter)"
default_value: 50
field: itermWindupPointPercent
min: 0
max: 90
- name: pid_iterm_limit_percent
description: "Limits max/min I-term value in stabilization PID controller. It solves the problem of servo saturation before take-off/throwing the airplane into the air. Or multirotors with low authority. By default, error accumulated in I-term can not exceed 33% of total pid throw (around 165us on deafult pidsum_limit of pitch/roll). Set 0 to disable completely."
default_value: 33
field: pidItermLimitPercent
min: 0
max: 200
- name: rate_accel_limit_roll_pitch
description: "Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting."
default_value: 0

View file

@ -79,6 +79,7 @@ typedef struct {
// Rate integrator
float errorGyroIf;
float errorGyroIfLimit;
// Used for ANGLE filtering (PT1, we don't need super-sharpness here)
pt1Filter_t angleFilterState;
@ -100,6 +101,7 @@ typedef struct {
#endif
uint16_t pidSumLimit;
filterApply4FnPtr ptermFilterApplyFn;
bool itermLimitActive;
bool itermFreezeActive;
pt3Filter_t rateTargetFilter;
@ -147,7 +149,6 @@ static EXTENDED_FASTRAM float dBoostMaxAtAlleceleration;
static EXTENDED_FASTRAM uint8_t yawLpfHz;
static EXTENDED_FASTRAM float motorItermWindupPoint;
static EXTENDED_FASTRAM float motorItermWindupPoint_inv;
static EXTENDED_FASTRAM float antiWindupScaler;
#ifdef USE_ANTIGRAVITY
static EXTENDED_FASTRAM float iTermAntigravityGain;
@ -263,8 +264,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.max_angle_inclination[FD_PITCH] = SETTING_MAX_ANGLE_INCLINATION_PIT_DEFAULT,
.pidSumLimit = SETTING_PIDSUM_LIMIT_DEFAULT,
.pidSumLimitYaw = SETTING_PIDSUM_LIMIT_YAW_DEFAULT,
.pidItermLimitPercent = SETTING_PID_ITERM_LIMIT_PERCENT_DEFAULT,
.fixedWingItermThrowLimit = SETTING_FW_ITERM_THROW_LIMIT_DEFAULT,
.fixedWingReferenceAirspeed = SETTING_FW_REFERENCE_AIRSPEED_DEFAULT,
.fixedWingCoordinatedYawGain = SETTING_FW_TURN_ASSIST_YAW_GAIN_DEFAULT,
.fixedWingCoordinatedPitchGain = SETTING_FW_TURN_ASSIST_PITCH_GAIN_DEFAULT,
@ -379,12 +380,14 @@ void pidResetErrorAccumulators(void)
// Reset R/P/Y integrator
for (int axis = 0; axis < 3; axis++) {
pidState[axis].errorGyroIf = 0.0f;
pidState[axis].errorGyroIfLimit = 0.0f;
}
}
void pidReduceErrorAccumulators(int8_t delta, uint8_t axis)
{
pidState[axis].errorGyroIf -= delta;
pidState[axis].errorGyroIfLimit -= delta;
}
float getTotalRateTarget(void)
@ -528,7 +531,7 @@ void updatePIDCoefficients(void)
pidState[axis].kT = 0.0f;
}
else {
const float axisTPA = (axis == FD_YAW && (!currentControlRateProfile->throttle.dynPID_on_YAW)) ? 1.0f : tpaFactor;
const float axisTPA = (axis == FD_YAW) ? 1.0f : tpaFactor;
pidState[axis].kP = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * axisTPA;
pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER;
pidState[axis].kD = pidBank()->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * axisTPA;
@ -619,7 +622,7 @@ static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynam
angleErrorDeg = DECIDEGREES_TO_DEGREES((float)angleFreefloatDeadband(DEGREES_TO_DECIDEGREES(navConfig()->fw.soaring_pitch_deadband), FD_PITCH));
if (!angleErrorDeg) {
pidState->errorGyroIf = 0.0f;
// pidState->errorGyroIfLimit = 0.0f;
pidState->errorGyroIfLimit = 0.0f;
}
}
@ -725,6 +728,14 @@ static float dTermProcess(pidState_t *pidState, float currentRateTarget, float d
return(newDTerm);
}
static void applyItermLimiting(pidState_t *pidState) {
if (pidState->itermLimitActive) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
} else
{
pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
}
}
static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv) {
UNUSED(pidState);
@ -749,9 +760,10 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
pidState->errorGyroIf += rateError * pidState->kI * dT;
}
if (pidProfile()->pidItermLimitPercent != 0){
float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent * 0.01f;
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit);
applyItermLimiting(pidState);
if (pidProfile()->fixedWingItermThrowLimit != 0) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit);
}
axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -pidState->pidSumLimit, +pidState->pidSumLimit);
@ -788,7 +800,7 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint,
const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
const float itermRelaxFactor = MAX(0, 1 - setpointHpf / (FLIGHT_MODE(ANGLE_MODE) ? (MC_ITERM_RELAX_SETPOINT_THRESHOLD * 0.2f):MC_ITERM_RELAX_SETPOINT_THRESHOLD));
const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD);
return itermErrorRate * itermRelaxFactor;
}
}
@ -817,30 +829,17 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm;
const float newOutputLimited = constrainf(newOutput, -pidState->pidSumLimit, +pidState->pidSumLimit);
float backCalc = newOutputLimited - newOutput;//back-calculation anti-windup
if (SIGN(backCalc) == SIGN(pidState->errorGyroIf)) {
//back calculation anti-windup can only shrink integrator, will not push it to the opposite direction
backCalc = 0.0f;
}
float itermErrorRate = applyItermRelax(axis, rateTarget, rateError);
#ifdef USE_ANTIGRAVITY
itermErrorRate *= iTermAntigravityGain;
#endif
if (!pidState->itermFreezeActive) {
pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT)
+ (backCalc * pidState->kT * antiWindupScaler * dT);
}
if (pidProfile()->pidItermLimitPercent != 0){
float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent * 0.01f;
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit);
}
pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT)
+ ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT);
// Don't grow I-term if motors are at their limit
// applyItermLimiting(pidState); //merged with itermFreezeActive
applyItermLimiting(pidState);
axisPID[axis] = newOutputLimited;
@ -1027,34 +1026,35 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng
pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT);
}
bool checkItermLimitingActive(pidState_t *pidState)
void checkItermLimitingActive(pidState_t *pidState)
{
bool shouldActivate=false;
bool shouldActivate;
if (usedPidControllerType == PID_TYPE_PIFF) {
shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition);
} else
{
shouldActivate = mixerIsOutputSaturated(); //just in case, since it is already managed by itermWindupPointPercent
shouldActivate = mixerIsOutputSaturated();
}
return STATE(ANTI_WINDUP) || shouldActivate;
pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate;
}
void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis)
{
bool shouldActivate=false;
if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) {
// Do not allow yaw I-term to grow when bank angle is too large
float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll);
if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){
shouldActivate |= true;
pidState->itermFreezeActive = true;
} else
{
shouldActivate |= false;
pidState->itermFreezeActive = false;
}
} else
{
pidState->itermFreezeActive = false;
}
shouldActivate |=checkItermLimitingActive(pidState);
pidState->itermFreezeActive = shouldActivate;
}
void FAST_CODE pidController(float dT)
@ -1134,14 +1134,15 @@ void FAST_CODE pidController(float dT)
pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees);
}
// Prevent Iterm accumulation when motors are near its saturation
antiWindupScaler = constrainf((1.0f - getMotorMixRange() * MAX(2*motorItermWindupPoint,1)) * motorItermWindupPoint_inv, 0.0f, 1.0f);
// Prevent strong Iterm accumulation during stick inputs
antiWindupScaler = constrainf((1.0f - getMotorMixRange()) * motorItermWindupPoint, 0.0f, 1.0f);
for (int axis = 0; axis < 3; axis++) {
// Apply setpoint rate of change limits
pidApplySetpointRateLimiting(&pidState[axis], axis, dT);
// Step 4: Run gyro-driven control
checkItermLimitingActive(&pidState[axis]);
checkItermFreezingActive(&pidState[axis], axis);
pidControllerApplyFn(&pidState[axis], axis, dT, dT_inv);
@ -1176,9 +1177,7 @@ void pidInit(void)
itermRelax = pidProfile()->iterm_relax;
yawLpfHz = pidProfile()->yaw_lpf_hz;
motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f);
motorItermWindupPoint_inv = 1.0f / motorItermWindupPoint;
motorItermWindupPoint = 1.0f / (1.0f - (pidProfile()->itermWindupPointPercent / 100.0f));
#ifdef USE_D_BOOST
dBoostMin = pidProfile()->dBoostMin;

View file

@ -31,6 +31,10 @@
#define HEADING_HOLD_RATE_LIMIT_MAX 250
#define HEADING_HOLD_RATE_LIMIT_DEFAULT 90
#define FW_ITERM_THROW_LIMIT_DEFAULT 165
#define FW_ITERM_THROW_LIMIT_MIN 0
#define FW_ITERM_THROW_LIMIT_MAX 500
#define AXIS_ACCEL_MIN_LIMIT 50
#define HEADING_HOLD_ERROR_LPF_FREQ 2
@ -117,9 +121,9 @@ typedef struct pidProfile_s {
uint16_t pidSumLimit;
uint16_t pidSumLimitYaw;
uint16_t pidItermLimitPercent;
// Airplane-specific parameters
uint16_t fixedWingItermThrowLimit;
float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned
float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn.
float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns.