Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00
Code Issues Wiki Activity

Made new PID loop improvements conditional, disabled some for F3 to save flash space.

Browse source
This commit is contained in:
mikeller 2018-06-03 17:07:10 +12:00
parent 9d2c7fd105
commit a9b3911e03
5 changed files with 71 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

68
src/main/flight/pid.c
View file

@ -203,10 +203,12 @@ static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn;
static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2[2];
static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn;
static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass;
#if defined(USE_ITERM_RELAX)
static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT][2];
static FAST_RAM_ZERO_INIT uint8_t itermRelax;
static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffLow;
static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffHigh;
#endif
void pidInitFilters(const pidProfile_t *pidProfile)
{
@ -283,11 +285,14 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
pt1FilterInit(&throttleLpf, pt1FilterGain(pidProfile->throttle_boost_cutoff, dT));
if (itermRelax)
#if defined(USE_ITERM_RELAX)
if (itermRelax) {
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
pt1FilterInit(&windupLpf[i][0], pt1FilterGain(itermRelaxCutoffLow, dT));
pt1FilterInit(&windupLpf[i][1], pt1FilterGain(itermRelaxCutoffHigh, dT));
}
}
#endif
}
typedef struct pidCoefficient_s {
@ -316,17 +321,23 @@ FAST_RAM_ZERO_INIT float throttleBoost;
pt1Filter_t throttleLpf;
static FAST_RAM_ZERO_INIT bool itermRotation;
#if defined(USE_SMART_FEEDFORWARD)
static FAST_RAM_ZERO_INIT bool smartFeedforward;
#endif
#if defined(USE_ABSOLUTE_CONTROL)
static FAST_RAM_ZERO_INIT float axisError[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float acGain;
static FAST_RAM_ZERO_INIT float acLimit;
static FAST_RAM_ZERO_INIT float acErrorLimit;
#endif
void pidResetITerm(void)
{
for (int axis = 0; axis < 3; axis++) {
pidData[axis].I = 0.0f;
#if defined(USE_ABSOLUTE_CONTROL)
axisError[axis] = 0.0f;
#endif
}
}
@ -374,10 +385,14 @@ void pidInitConfig(const pidProfile_t *pidProfile)
itermLimit = pidProfile->itermLimit;
throttleBoost = pidProfile->throttle_boost * 0.1f;
itermRotation = pidProfile->iterm_rotation;
#if defined(USE_SMART_FEEDFORWARD)
smartFeedforward = pidProfile->smart_feedforward;
#endif
#if defined(USE_ITERM_RELAX)
itermRelax = pidProfile->iterm_relax;
itermRelaxCutoffLow = pidProfile->iterm_relax_cutoff_low;
itermRelaxCutoffHigh = pidProfile->iterm_relax_cutoff_high;
#endif
#ifdef USE_ACRO_TRAINER
acroTrainerAngleLimit = pidProfile->acro_trainer_angle_limit;
@ -386,9 +401,11 @@ void pidInitConfig(const pidProfile_t *pidProfile)
acroTrainerGain = (float)pidProfile->acro_trainer_gain / 10.0f;
#endif // USE_ACRO_TRAINER
#if defined(USE_ABSOLUTE_CONTROL)
acGain = (float)pidProfile->abs_control_gain;
acLimit = (float)pidProfile->abs_control_limit;
acErrorLimit = (float)pidProfile->abs_control_error_limit;
#endif
}
void pidInit(const pidProfile_t *pidProfile)
@ -591,15 +608,21 @@ static void rotateVector(float v[XYZ_AXIS_COUNT], float rotation[XYZ_AXIS_COUNT]
static void rotateITermAndAxisError()
{
if (itermRotation || acGain > 0) {
if (itermRotation
#if defined(USE_ABSOLUTE_CONTROL)
|| acGain > 0
#endif
) {
const float gyroToAngle = dT * RAD;
float rotationRads[3];
for (int i = FD_ROLL; i <= FD_YAW; i++) {
rotationRads[i] = gyro.gyroADCf[i] * gyroToAngle;
}
#if defined(USE_ABSOLUTE_CONTROL)
if (acGain > 0) {
rotateVector(axisError, rotationRads);
}
#endif
if (itermRotation) {
float v[3];
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
@ -743,13 +766,14 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
}
#endif // USE_YAW_SPIN_RECOVERY
#if defined(USE_ABSOLUTE_CONTROL)
// mix in a correction for accrued attitude error
float stickSetpoint = currentPidSetpoint;
float acCorrection = 0;
if (acGain > 0) {
acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit);
currentPidSetpoint += acCorrection;
}
#endif
// -----calculate error rate
const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
@ -770,11 +794,18 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
}
// -----calculate I component
float itermErrorRate;
float itermErrorRate = 0.0f;
#if defined(USE_ABSOLUTE_CONTROL)
float acErrorRate;
#endif
#if defined(USE_ITERM_RELAX)
if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) {
const float gyroTargetLow = pt1FilterApply(&windupLpf[axis][0], stickSetpoint) + acCorrection;
const float gyroTargetHigh = pt1FilterApply(&windupLpf[axis][1], stickSetpoint) + acCorrection;
float gyroTargetLow = pt1FilterApply(&windupLpf[axis][0], currentPidSetpoint);
float gyroTargetHigh = pt1FilterApply(&windupLpf[axis][1], currentPidSetpoint);
#if defined(USE_ABSOLUTE_CONTROL)
gyroTargetLow += acCorrection;
gyroTargetHigh += acCorrection;
#endif
if (axis < FD_YAW) {
int itemOffset = (axis << 1);
DEBUG_SET(DEBUG_ITERM_RELAX, itemOffset++, gyroTargetHigh);
@ -782,19 +813,29 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
}
const float gmax = MAX(gyroTargetHigh, gyroTargetLow);
const float gmin = MIN(gyroTargetHigh, gyroTargetLow);
if (gyroRate >= gmin && gyroRate <= gmax) {
itermErrorRate = acCorrection;
} else {
if (gyroRate < gmin || gyroRate > gmax) {
itermErrorRate = (gyroRate > gmax ? gmax : gmin ) - gyroRate;
}
#if defined(USE_ABSOLUTE_CONTROL)
else {
itermErrorRate = acCorrection;
}
acErrorRate = itermErrorRate - acCorrection;
} else {
itermErrorRate = acErrorRate = errorRate;
#endif
} else
#endif // USE_ITERM_RELAX
{
itermErrorRate = errorRate;
#if defined(USE_ABSOLUTE_CONTROL)
acErrorRate = errorRate;
#endif
}
#if defined(USE_ABSOLUTE_CONTROL)
if (acGain > 0) {
axisError[axis] = constrainf(axisError[axis] + acErrorRate * dT, -acErrorLimit, acErrorLimit);
}
#endif
const float ITerm = pidData[axis].I;
const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);
@ -824,6 +865,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
const float pidFeedForward =
pidCoefficient[axis].Kd * dynCd * transition *
(currentPidSetpoint - previousPidSetpoint[axis]) * tpaFactor / dT;
#if defined(USE_SMART_FEEDFORWARD)
bool addFeedforward = true;
if (smartFeedforward) {
if (pidData[axis].P * pidFeedForward > 0) {
@ -835,7 +877,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
}
}
}
if (addFeedforward) {
if (addFeedforward)
#endif
{
pidData[axis].D += pidFeedForward;
}
previousGyroRateDterm[axis] = gyroRateDterm[axis];

2
src/main/flight/pid.h
View file

@ -83,8 +83,6 @@ typedef enum
ITERM_RELAX_RPY
} itermRelax_e;
typedef struct pidProfile_s {
uint16_t yaw_lowpass_hz; // Additional yaw filter when yaw axis too noisy
uint16_t dterm_lowpass_hz; // Delta Filter in hz

10
src/main/interface/settings.c
View file

@ -337,9 +337,11 @@ static const char * const lookupTableVideoSystem[] = {
};
#endif // USE_MAX7456
#if defined(USE_ITERM_RELAX)
static const char * const lookupTableItermRelax[] = {
"OFF", "RP", "RPY"
};
#endif
#ifdef USE_ACRO_TRAINER
static const char * const lookupTableAcroTrainerDebug[] = {
@ -422,7 +424,9 @@ const lookupTableEntry_t lookupTables[] = {
#ifdef USE_MAX7456
LOOKUP_TABLE_ENTRY(lookupTableVideoSystem),
#endif // USE_MAX7456
#if defined(USE_ITERM_RELAX)
LOOKUP_TABLE_ENTRY(lookupTableItermRelax),
#endif
#ifdef USE_ACRO_TRAINER
LOOKUP_TABLE_ENTRY(lookupTableAcroTrainerDebug),
#endif // USE_ACRO_TRAINER
@ -760,8 +764,12 @@ const clivalue_t valueTable[] = {
{ "crash_recovery", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRASH_RECOVERY }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery) },
{ "iterm_rotation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_rotation) },
#if defined(USE_SMART_FEEDFORWARD)
{ "smart_feedforward", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, smart_feedforward) },
#endif
#if defined(USE_ITERM_RELAX)
{ "iterm_relax", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax) },
#endif
{ "iterm_relax_cutoff_low", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff_low) },
{ "iterm_relax_cutoff_high", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff_high) },
{ "iterm_windup", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) },
@ -799,9 +807,11 @@ const clivalue_t valueTable[] = {
{ "horizon_tilt_effect", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_effect) },
{ "horizon_tilt_expert_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_expert_mode) },
#if defined(USE_ABSOLUTE_CONTROL)
{ "abs_control_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_gain) },
{ "abs_control_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_limit) },
{ "abs_control_error_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 45 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_error_limit) },
#endif
// PG_TELEMETRY_CONFIG

2
src/main/interface/settings.h
View file

@ -98,7 +98,9 @@ typedef enum {
#ifdef USE_MAX7456
TABLE_VIDEO_SYSTEM,
#endif // USE_MAX7456
#if defined(USE_ITERM_RELAX)
TABLE_ITERM_RELAX,
#endif
#ifdef USE_ACRO_TRAINER
TABLE_ACRO_TRAINER_DEBUG,
#endif // USE_ACRO_TRAINER

3
src/main/target/common_fc_pre.h
View file

@ -188,6 +188,7 @@
#define USE_ESC_SENSOR_INFO
#define USE_CRSF_CMS_TELEMETRY
#define USE_BOARD_INFO
#define USE_SMART_FEEDFORWARD
#ifdef USE_SERIALRX_SPEKTRUM
#define USE_SPEKTRUM_BIND
@ -216,4 +217,6 @@
#define USE_TELEMETRY_MAVLINK
#define USE_UNCOMMON_MIXERS
#define USE_SIGNATURE
#define USE_ITERM_RELAX
#define USE_ABSOLUTE_CONTROL
#endif