mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-19 06:15:16 +03:00
Add ITerm anti-windup based on motor output saturation
Added noise threshold for PID ITerm accumulation Removed ITerm setpoint scaler. Added CLI and MSP settings Made default ITerm noise threshold zero. Added CLI setting. Removed itermWindupPointPercent from MSP
This commit is contained in:
Martin Budden
borisbstyle
parent
990c13b7ea
commit
9dfb3e45ee
8 changed files with 39 additions and 22 deletions
|
|
@ -1250,8 +1250,7 @@ static bool blackboxWriteSysinfo()
|
|||
BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", currentProfile->pidProfile.yaw_lpf_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz:%d", currentProfile->pidProfile.dterm_notch_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff:%d", currentProfile->pidProfile.dterm_notch_cutoff);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rollPitchItermIgnoreRate:%d", currentProfile->pidProfile.rollPitchItermIgnoreRate);
|
||||
BLACKBOX_PRINT_HEADER_LINE("yawItermIgnoreRate:%d", currentProfile->pidProfile.yawItermIgnoreRate);
|
||||
BLACKBOX_PRINT_HEADER_LINE("itermWindupPointPercent:%d", currentProfile->pidProfile.itermWindupPointPercent);
|
||||
BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", currentProfile->pidProfile.yaw_p_limit);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dterm_average_count:%d", currentProfile->pidProfile.dterm_average_count);
|
||||
BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", currentProfile->pidProfile.vbatPidCompensation);
|
||||
|
|
|
|||
|
|
@ -707,8 +707,8 @@ const clivalue_t valueTable[] = {
|
|||
{ "yaw_accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0.1f, 50.0f } },
|
||||
{ "accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0.1f, 50.0f } },
|
||||
|
||||
{ "accum_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } },
|
||||
{ "yaw_accum_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } },
|
||||
{ "iterm_windup", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermWindupPointPercent, .config.minmax = {50, 100 } },
|
||||
{ "iterm_noise_threshold", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermNoiseThreshold, .config.minmax = {0, 20 } },
|
||||
{ "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } },
|
||||
{ "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &pidConfig()->pid_process_denom, .config.minmax = { 1, 16 } },
|
||||
|
||||
|
|
|
|||
|
|
@ -169,8 +169,8 @@ static void resetPidProfile(pidProfile_t *pidProfile)
|
|||
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
|
||||
pidProfile->pidSumLimit = PIDSUM_LIMIT;
|
||||
pidProfile->yaw_lpf_hz = 0;
|
||||
pidProfile->rollPitchItermIgnoreRate = 200;
|
||||
pidProfile->yawItermIgnoreRate = 55;
|
||||
pidProfile->itermWindupPointPercent = 75;
|
||||
pidProfile->itermNoiseThreshold = 0;
|
||||
pidProfile->dterm_filter_type = FILTER_BIQUAD;
|
||||
pidProfile->dterm_lpf_hz = 100; // filtering ON by default
|
||||
pidProfile->dterm_notch_hz = 260;
|
||||
|
|
|
|||
|
|
@ -1159,8 +1159,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
break;
|
||||
|
||||
case MSP_PID_ADVANCED:
|
||||
sbufWriteU16(dst, currentProfile->pidProfile.rollPitchItermIgnoreRate);
|
||||
sbufWriteU16(dst, currentProfile->pidProfile.yawItermIgnoreRate);
|
||||
sbufWriteU16(dst, 0);
|
||||
sbufWriteU16(dst, 0);
|
||||
sbufWriteU16(dst, currentProfile->pidProfile.yaw_p_limit);
|
||||
sbufWriteU8(dst, 0); // reserved
|
||||
sbufWriteU8(dst, currentProfile->pidProfile.vbatPidCompensation);
|
||||
|
|
@ -1515,8 +1515,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
break;
|
||||
|
||||
case MSP_SET_PID_ADVANCED:
|
||||
currentProfile->pidProfile.rollPitchItermIgnoreRate = sbufReadU16(src);
|
||||
currentProfile->pidProfile.yawItermIgnoreRate = sbufReadU16(src);
|
||||
sbufReadU16(src);
|
||||
sbufReadU16(src);
|
||||
currentProfile->pidProfile.yaw_p_limit = sbufReadU16(src);
|
||||
sbufReadU8(src); // reserved
|
||||
currentProfile->pidProfile.vbatPidCompensation = sbufReadU8(src);
|
||||
|
|
|
|||
|
|
@ -56,6 +56,7 @@
|
|||
#define EXTERNAL_CONVERSION_MAX_VALUE 2000
|
||||
|
||||
static uint8_t motorCount;
|
||||
static float motorMixRange;
|
||||
|
||||
int16_t motor[MAX_SUPPORTED_MOTORS];
|
||||
int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
||||
|
|
@ -245,6 +246,11 @@ uint8_t getMotorCount()
|
|||
return motorCount;
|
||||
}
|
||||
|
||||
float getMotorMixRange()
|
||||
{
|
||||
return motorMixRange;
|
||||
}
|
||||
|
||||
bool isMotorProtocolDshot(void) {
|
||||
#ifdef USE_DSHOT
|
||||
switch(motorConfig->motorPwmProtocol) {
|
||||
|
|
@ -500,7 +506,7 @@ void mixTable(pidProfile_t *pidProfile)
|
|||
}
|
||||
}
|
||||
|
||||
const float motorMixRange = motorMixMax - motorMixMin;
|
||||
motorMixRange = motorMixMax - motorMixMin;
|
||||
|
||||
if (motorMixRange > 1.0f) {
|
||||
for (int i = 0; i < motorCount; i++) {
|
||||
|
|
|
|||
|
|
@ -113,10 +113,12 @@ typedef struct airplaneConfig_s {
|
|||
extern const mixer_t mixers[];
|
||||
extern int16_t motor[MAX_SUPPORTED_MOTORS];
|
||||
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
||||
|
||||
struct motorConfig_s;
|
||||
struct rxConfig_s;
|
||||
|
||||
uint8_t getMotorCount();
|
||||
float getMotorMixRange();
|
||||
|
||||
void mixerUseConfigs(
|
||||
flight3DConfig_t *flight3DConfigToUse,
|
||||
|
|
|
|||
|
|
@ -34,6 +34,7 @@
|
|||
|
||||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/navigation.h"
|
||||
|
||||
#include "sensors/gyro.h"
|
||||
|
|
@ -146,7 +147,8 @@ void pidInitFilters(const pidProfile_t *pidProfile)
|
|||
}
|
||||
}
|
||||
|
||||
static float Kp[3], Ki[3], Kd[3], c[3], levelGain, horizonGain, horizonTransition, maxVelocity[3], relaxFactor[3];
|
||||
static float Kp[3], Ki[3], Kd[3], c[3], maxVelocity[3], relaxFactor[3];
|
||||
static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermNoiseThresholdDps;
|
||||
|
||||
void pidInitConfig(const pidProfile_t *pidProfile) {
|
||||
for(int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
||||
|
|
@ -161,6 +163,8 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
|
|||
horizonTransition = 100.0f / pidProfile->D8[PIDLEVEL];
|
||||
maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 1000 * dT;
|
||||
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT;
|
||||
ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
|
||||
ITermNoiseThresholdDps = (float)pidProfile->itermNoiseThreshold / 10.0f;
|
||||
}
|
||||
|
||||
static float calcHorizonLevelStrength(void) {
|
||||
|
|
@ -238,15 +242,21 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
|
|||
}
|
||||
|
||||
// -----calculate I component
|
||||
// Reduce strong Iterm accumulation during higher stick inputs
|
||||
const float accumulationThreshold = (axis == FD_YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
|
||||
const float setpointRateScaler = constrainf(1.0f - (ABS(currentPidSetpoint) / accumulationThreshold), 0.0f, 1.0f);
|
||||
|
||||
float ITerm = previousGyroIf[axis];
|
||||
ITerm += Ki[axis] * errorRate * dT * setpointRateScaler * itermAccelerator;
|
||||
// limit maximum integrator value to prevent WindUp
|
||||
ITerm = constrainf(ITerm, -250.0f, 250.0f);
|
||||
previousGyroIf[axis] = ITerm;
|
||||
const float motorMixRangeAbs = ABS(getMotorMixRange());
|
||||
if (motorMixRangeAbs < 1.0f && (errorRate > ITermNoiseThresholdDps || errorRate < -ITermNoiseThresholdDps)) {
|
||||
// Only increase ITerm if motor output is not saturated and errorRate exceeds noise threshold
|
||||
// Reduce strong Iterm accumulation during higher stick inputs
|
||||
float ITermDelta = Ki[axis] * errorRate * dT * itermAccelerator;
|
||||
// gradually scale back integration when above windup point (default is 75%)
|
||||
if (motorMixRangeAbs > ITermWindupPoint) {
|
||||
ITermDelta *= (1.0f - motorMixRangeAbs) / (1.0f - ITermWindupPoint);
|
||||
}
|
||||
ITerm += ITermDelta;
|
||||
// also limit maximum integrator value to prevent windup
|
||||
ITerm = constrainf(ITerm, -250.0f, 250.0f);
|
||||
previousGyroIf[axis] = ITerm;
|
||||
}
|
||||
|
||||
// -----calculate D component (Yaw D not yet supported)
|
||||
float DTerm = 0.0;
|
||||
|
|
|
|||
|
|
@ -66,8 +66,8 @@ typedef struct pidProfile_s {
|
|||
uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy
|
||||
uint16_t dterm_notch_hz; // Biquad dterm notch hz
|
||||
uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff
|
||||
uint16_t rollPitchItermIgnoreRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates
|
||||
uint16_t yawItermIgnoreRate; // Experimental threshold for resetting iterm for yaw on certain rates
|
||||
uint8_t itermWindupPointPercent; // Experimental ITerm windup threshold, percent motor saturation
|
||||
uint8_t itermNoiseThreshold; // Experimental ITerm noise threshold
|
||||
uint16_t yaw_p_limit;
|
||||
float pidSumLimit;
|
||||
uint8_t dterm_average_count; // Configurable delta count for dterm
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue