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

PID controller feedforward

Browse source 
Restructures the PID controller to decouple feedforward from D.

Cleaned up the structure of the PID controller; moved some feature-based enhancements out of the main structure.

Feedforward becomes a separate component of the PID controller and there is now:
f_pitch
f_roll
f_yaw

The default values of 60 for pitch and roll matches the default setpoint weight used in BF3.4.  Yaw previously had no setpoint weight capability so the default here needs to be discussed.  Currently it's also set to 60 and flight testing seems positive.  Feedforward on yaw adds a lot of value so I don't think we want to default to 0.  Instead we need decide on the default.

All occurences of setpoint weight have been replaced by feedforward. "setpoint_relax_ratio" has been renamed to "feedforward_transition".

The pidSum now consists of P + I + D + F.

D has been added back for yaw (disabled by default with d_yaw = 0). We've found little need for D for normal quads but it may have value for other configurations - particularly tricopters.

Updated CMS menus to support adjusting the feedforward for each axis.

Changed the default for "rc_interp_ch" to be "RPYT".  Need yaw to be smoothed to support feedforward.

Open issues:

Needs BFC support
- Need to add support for the axis "F" gains.
- Remove "setpoint weight" slider.
- Rename "D Setpoint transition" to "Feedforward transition"

Needs BBE support
- Header "setpoint_relaxation_ratio" has been renamed "feedforward_transition"
- Header "dterm_setpoint_weight" has been replaced with an array named "feed_forward_weight".
  example: H feed_forward_weight:65,60,60    (R,P,Y)
- PID component "AXISF" has been added for all axes. Should be handled like P, I and D values.
- PidSum calculation needs to include F.

Needs LUA script support
- Support the renamed "setpoint_relax_ratio".
- Support for feedforward weight on all 3 axes.

Open code issues:
- rc_adjustments.c - support for adjusting feedforward weight for all axes. Currently only supporting roll - needs coordination with BFC.
This commit is contained in:
Bruce Luckcuck 2018-06-24 08:49:28 -04:00
parent 625b23915e
commit 17e76e48f6
13 changed files with 201 additions and 145 deletions
Download patch file Download diff file Expand all files Collapse all files

18
src/main/blackbox/blackbox.c
View file

@ -182,6 +182,9 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
{"axisD", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_0)}, {"axisD", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_0)},
{"axisD", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_1)}, {"axisD", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_1)},
{"axisD", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_2)}, {"axisD", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_2)},
{"axisF", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"axisF", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"axisF", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
/* rcCommands are encoded together as a group in P-frames: */ /* rcCommands are encoded together as a group in P-frames: */
{"rcCommand", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)}, {"rcCommand", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"rcCommand", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)}, {"rcCommand", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
@ -285,6 +288,7 @@ typedef struct blackboxMainState_s {
int32_t axisPID_P[XYZ_AXIS_COUNT]; int32_t axisPID_P[XYZ_AXIS_COUNT];
int32_t axisPID_I[XYZ_AXIS_COUNT]; int32_t axisPID_I[XYZ_AXIS_COUNT];
int32_t axisPID_D[XYZ_AXIS_COUNT]; int32_t axisPID_D[XYZ_AXIS_COUNT];
int32_t axisPID_F[XYZ_AXIS_COUNT];
int16_t rcCommand[4]; int16_t rcCommand[4];
int16_t gyroADC[XYZ_AXIS_COUNT]; int16_t gyroADC[XYZ_AXIS_COUNT];
@ -531,6 +535,8 @@ static void writeIntraframe(void)
} }
} }
blackboxWriteSignedVBArray(blackboxCurrent->axisPID_F, XYZ_AXIS_COUNT);
// Write roll, pitch and yaw first: // Write roll, pitch and yaw first:
blackboxWriteSigned16VBArray(blackboxCurrent->rcCommand, 3); blackboxWriteSigned16VBArray(blackboxCurrent->rcCommand, 3);
@ -662,6 +668,9 @@ static void writeInterframe(void)
} }
} }
arraySubInt32(deltas, blackboxCurrent->axisPID_F, blackboxLast->axisPID_F, XYZ_AXIS_COUNT);
blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
/* /*
* RC tends to stay the same or fairly small for many frames at a time, so use an encoding that * RC tends to stay the same or fairly small for many frames at a time, so use an encoding that
* can pack multiple values per byte: * can pack multiple values per byte:
@ -992,6 +1001,7 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->axisPID_P[i] = pidData[i].P; blackboxCurrent->axisPID_P[i] = pidData[i].P;
blackboxCurrent->axisPID_I[i] = pidData[i].I; blackboxCurrent->axisPID_I[i] = pidData[i].I;
blackboxCurrent->axisPID_D[i] = pidData[i].D; blackboxCurrent->axisPID_D[i] = pidData[i].D;
blackboxCurrent->axisPID_F[i] = pidData[i].F;
blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]); blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
blackboxCurrent->accADC[i] = lrintf(acc.accADC[i]); blackboxCurrent->accADC[i] = lrintf(acc.accADC[i]);
#ifdef USE_MAG #ifdef USE_MAG
@ -1286,8 +1296,12 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("anti_gravity_mode", "%d", currentPidProfile->antiGravityMode); BLACKBOX_PRINT_HEADER_LINE("anti_gravity_mode", "%d", currentPidProfile->antiGravityMode);
BLACKBOX_PRINT_HEADER_LINE("anti_gravity_threshold", "%d", currentPidProfile->itermThrottleThreshold); BLACKBOX_PRINT_HEADER_LINE("anti_gravity_threshold", "%d", currentPidProfile->itermThrottleThreshold);
BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain", "%d", currentPidProfile->itermAcceleratorGain); BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain", "%d", currentPidProfile->itermAcceleratorGain);
BLACKBOX_PRINT_HEADER_LINE("setpoint_relaxation_ratio", "%d", currentPidProfile->setpointRelaxRatio);
BLACKBOX_PRINT_HEADER_LINE("dterm_setpoint_weight", "%d", currentPidProfile->dtermSetpointWeight); BLACKBOX_PRINT_HEADER_LINE("feedforward_transition", "%d", currentPidProfile->feedForwardTransition);
BLACKBOX_PRINT_HEADER_LINE("feedforward_weight", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].F,
currentPidProfile->pid[PID_PITCH].F,
currentPidProfile->pid[PID_YAW].F);
BLACKBOX_PRINT_HEADER_LINE("acc_limit_yaw", "%d", currentPidProfile->yawRateAccelLimit); BLACKBOX_PRINT_HEADER_LINE("acc_limit_yaw", "%d", currentPidProfile->yawRateAccelLimit);
BLACKBOX_PRINT_HEADER_LINE("acc_limit", "%d", currentPidProfile->rateAccelLimit); BLACKBOX_PRINT_HEADER_LINE("acc_limit", "%d", currentPidProfile->rateAccelLimit);
BLACKBOX_PRINT_HEADER_LINE("pidsum_limit", "%d", currentPidProfile->pidSumLimit); BLACKBOX_PRINT_HEADER_LINE("pidsum_limit", "%d", currentPidProfile->pidSumLimit);

19
src/main/cms/cms_menu_imu.c
View file

@ -58,6 +58,7 @@ static uint8_t tmpPidProfileIndex;
static uint8_t pidProfileIndex; static uint8_t pidProfileIndex;
static char pidProfileIndexString[] = " p"; static char pidProfileIndexString[] = " p";
static uint8_t tempPid[3][3]; static uint8_t tempPid[3][3];
static uint16_t tempPidF[3];
static uint8_t tmpRateProfileIndex; static uint8_t tmpRateProfileIndex;
static uint8_t rateProfileIndex; static uint8_t rateProfileIndex;
@ -115,6 +116,7 @@ static long cmsx_PidRead(void)
tempPid[i][0] = pidProfile->pid[i].P; tempPid[i][0] = pidProfile->pid[i].P;
tempPid[i][1] = pidProfile->pid[i].I; tempPid[i][1] = pidProfile->pid[i].I;
tempPid[i][2] = pidProfile->pid[i].D; tempPid[i][2] = pidProfile->pid[i].D;
tempPidF[i] = pidProfile->pid[i].F;
} }
return 0; return 0;
@ -137,6 +139,7 @@ static long cmsx_PidWriteback(const OSD_Entry *self)
pidProfile->pid[i].P = tempPid[i][0]; pidProfile->pid[i].P = tempPid[i][0];
pidProfile->pid[i].I = tempPid[i][1]; pidProfile->pid[i].I = tempPid[i][1];
pidProfile->pid[i].D = tempPid[i][2]; pidProfile->pid[i].D = tempPid[i][2];
pidProfile->pid[i].F = tempPidF[i];
} }
pidInitConfig(currentPidProfile); pidInitConfig(currentPidProfile);
@ -150,14 +153,17 @@ static OSD_Entry cmsx_menuPidEntries[] =
{ "ROLL P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][0], 0, 200, 1 }, 0 }, { "ROLL P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][0], 0, 200, 1 }, 0 },
{ "ROLL I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][1], 0, 200, 1 }, 0 }, { "ROLL I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][1], 0, 200, 1 }, 0 },
{ "ROLL D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][2], 0, 200, 1 }, 0 }, { "ROLL D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][2], 0, 200, 1 }, 0 },
{ "ROLL F", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidF[PID_ROLL], 0, 2000, 1 }, 0 },
{ "PITCH P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][0], 0, 200, 1 }, 0 }, { "PITCH P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][0], 0, 200, 1 }, 0 },
{ "PITCH I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][1], 0, 200, 1 }, 0 }, { "PITCH I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][1], 0, 200, 1 }, 0 },
{ "PITCH D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][2], 0, 200, 1 }, 0 }, { "PITCH D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][2], 0, 200, 1 }, 0 },
{ "PITCH F", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidF[PID_PITCH], 0, 2000, 1 }, 0 },
{ "YAW P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][0], 0, 200, 1 }, 0 }, { "YAW P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][0], 0, 200, 1 }, 0 },
{ "YAW I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][1], 0, 200, 1 }, 0 }, { "YAW I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][1], 0, 200, 1 }, 0 },
{ "YAW D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][2], 0, 200, 1 }, 0 }, { "YAW D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][2], 0, 200, 1 }, 0 },
{ "YAW F", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidF[PID_YAW], 0, 2000, 1 }, 0 },
{ "BACK", OME_Back, NULL, NULL, 0 }, { "BACK", OME_Back, NULL, NULL, 0 },
{ NULL, OME_END, NULL, NULL, 0 } { NULL, OME_END, NULL, NULL, 0 }
@ -237,8 +243,7 @@ static CMS_Menu cmsx_menuRateProfile = {
.entries = cmsx_menuRateProfileEntries .entries = cmsx_menuRateProfileEntries
}; };
static uint16_t cmsx_dtermSetpointWeight; static uint8_t cmsx_feedForwardTransition;
static uint8_t cmsx_setpointRelaxRatio;
static uint8_t cmsx_angleStrength; static uint8_t cmsx_angleStrength;
static uint8_t cmsx_horizonStrength; static uint8_t cmsx_horizonStrength;
static uint8_t cmsx_horizonTransition; static uint8_t cmsx_horizonTransition;
@ -250,8 +255,8 @@ static long cmsx_profileOtherOnEnter(void)
pidProfileIndexString[1] = '0' + tmpPidProfileIndex; pidProfileIndexString[1] = '0' + tmpPidProfileIndex;
const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex); const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
cmsx_dtermSetpointWeight = pidProfile->dtermSetpointWeight;
cmsx_setpointRelaxRatio = pidProfile->setpointRelaxRatio; cmsx_feedForwardTransition = pidProfile->feedForwardTransition;
cmsx_angleStrength = pidProfile->pid[PID_LEVEL].P; cmsx_angleStrength = pidProfile->pid[PID_LEVEL].P;
cmsx_horizonStrength = pidProfile->pid[PID_LEVEL].I; cmsx_horizonStrength = pidProfile->pid[PID_LEVEL].I;
@ -268,8 +273,7 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self)
UNUSED(self); UNUSED(self);
pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
pidProfile->dtermSetpointWeight = cmsx_dtermSetpointWeight; pidProfile->feedForwardTransition = cmsx_feedForwardTransition;
pidProfile->setpointRelaxRatio = cmsx_setpointRelaxRatio;
pidInitConfig(currentPidProfile); pidInitConfig(currentPidProfile);
pidProfile->pid[PID_LEVEL].P = cmsx_angleStrength; pidProfile->pid[PID_LEVEL].P = cmsx_angleStrength;
@ -285,8 +289,7 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self)
static OSD_Entry cmsx_menuProfileOtherEntries[] = { static OSD_Entry cmsx_menuProfileOtherEntries[] = {
{ "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString, 0 }, { "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString, 0 },
{ "D SETPT WT", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_dtermSetpointWeight, 0, 2000, 1 }, 0 }, { "FF TRANS", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_feedForwardTransition, 0, 100, 1, 10 }, 0 },
{ "SETPT TRS", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_setpointRelaxRatio, 0, 100, 1, 10 }, 0 },
{ "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } , 0 }, { "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } , 0 },
{ "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonStrength, 0, 200, 1 } , 0 }, { "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonStrength, 0, 200, 1 } , 0 },
{ "HORZN TRS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonTransition, 0, 200, 1 } , 0 }, { "HORZN TRS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonTransition, 0, 200, 1 } , 0 },

12
src/main/fc/config.c
View file

@ -263,10 +263,20 @@ static void validateAndFixConfig(void)
if (!rcSmoothingIsEnabled() || rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_T) { if (!rcSmoothingIsEnabled() || rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_T) {
for (unsigned i = 0; i < MAX_PROFILE_COUNT; i++) { for (unsigned i = 0; i < MAX_PROFILE_COUNT; i++) {
pidProfilesMutable(i)->dtermSetpointWeight = 0; pidProfilesMutable(i)->pid[PID_ROLL].F = 0;
pidProfilesMutable(i)->pid[PID_PITCH].F = 0;
} }
} }
if (!rcSmoothingIsEnabled() ||
(rxConfig()->rcInterpolationChannels != INTERPOLATION_CHANNELS_RPY &&
rxConfig()->rcInterpolationChannels != INTERPOLATION_CHANNELS_RPYT)) {
for (unsigned i = 0; i < MAX_PROFILE_COUNT; i++) {
pidProfilesMutable(i)->pid[PID_YAW].F = 0;
}
}
#if defined(USE_THROTTLE_BOOST) #if defined(USE_THROTTLE_BOOST)
if (!rcSmoothingIsEnabled() || if (!rcSmoothingIsEnabled() ||
!(rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_RPYT !(rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_RPYT

12
src/main/fc/rc_adjustments.c
View file

@ -406,13 +406,13 @@ static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t a
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue);
break; break;
case ADJUSTMENT_D_SETPOINT: case ADJUSTMENT_D_SETPOINT:
newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 2000); // FIXME magic numbers repeated in cli.c newValue = constrain((int)pidProfile->pid[PID_ROLL].F + delta, 0, 2000); // FIXME magic numbers repeated in cli.c
pidProfile->dtermSetpointWeight = newValue; pidProfile->pid[PID_ROLL].F = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue);
break; break;
case ADJUSTMENT_D_SETPOINT_TRANSITION: case ADJUSTMENT_D_SETPOINT_TRANSITION:
newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 1, 100); // FIXME magic numbers repeated in cli.c newValue = constrain((int)pidProfile->feedForwardTransition + delta, 1, 100); // FIXME magic numbers repeated in cli.c
pidProfile->setpointRelaxRatio = newValue; pidProfile->feedForwardTransition = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue);
break; break;
default: default:
@ -556,12 +556,12 @@ static int applyAbsoluteAdjustment(controlRateConfig_t *controlRateConfig, adjus
break; break;
case ADJUSTMENT_D_SETPOINT: case ADJUSTMENT_D_SETPOINT:
newValue = constrain(value, 0, 2000); // FIXME magic numbers repeated in cli.c newValue = constrain(value, 0, 2000); // FIXME magic numbers repeated in cli.c
pidProfile->dtermSetpointWeight = newValue; pidProfile->pid[PID_ROLL].F = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue);
break; break;
case ADJUSTMENT_D_SETPOINT_TRANSITION: case ADJUSTMENT_D_SETPOINT_TRANSITION:
newValue = constrain(value, 1, 100); // FIXME magic numbers repeated in cli.c newValue = constrain(value, 1, 100); // FIXME magic numbers repeated in cli.c
pidProfile->setpointRelaxRatio = newValue; pidProfile->feedForwardTransition = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue);
break; break;
default: default:

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

@ -104,22 +104,22 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
#define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff #define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 4); PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 5);
void resetPidProfile(pidProfile_t *pidProfile) void resetPidProfile(pidProfile_t *pidProfile)
{ {
RESET_CONFIG(pidProfile_t, pidProfile, RESET_CONFIG(pidProfile_t, pidProfile,
.pid = { .pid = {
[PID_ROLL] = { 46, 45, 25 }, [PID_ROLL] = { 46, 45, 25, 60 },
[PID_PITCH] = { 50, 50, 27 }, [PID_PITCH] = { 50, 50, 27, 60 },
[PID_YAW] = { 65, 45, 0 }, [PID_YAW] = { 65, 45, 0 , 60 },
[PID_ALT] = { 50, 0, 0 }, [PID_ALT] = { 50, 0, 0, 0 },
[PID_POS] = { 15, 0, 0 }, // POSHOLD_P * 100, POSHOLD_I * 100, [PID_POS] = { 15, 0, 0, 0 }, // POSHOLD_P * 100, POSHOLD_I * 100,
[PID_POSR] = { 34, 14, 53 }, // POSHOLD_RATE_P * 10, POSHOLD_RATE_I * 100, POSHOLD_RATE_D * 1000, [PID_POSR] = { 34, 14, 53, 0 }, // POSHOLD_RATE_P * 10, POSHOLD_RATE_I * 100, POSHOLD_RATE_D * 1000,
[PID_NAVR] = { 25, 33, 83 }, // NAV_P * 10, NAV_I * 100, NAV_D * 1000 [PID_NAVR] = { 25, 33, 83, 0 }, // NAV_P * 10, NAV_I * 100, NAV_D * 1000
[PID_LEVEL] = { 50, 50, 75 }, [PID_LEVEL] = { 50, 50, 75, 0 },
[PID_MAG] = { 40, 0, 0 }, [PID_MAG] = { 40, 0, 0, 0 },
[PID_VEL] = { 55, 55, 75 } [PID_VEL] = { 55, 55, 75, 0 }
}, },
.pidSumLimit = PIDSUM_LIMIT, .pidSumLimit = PIDSUM_LIMIT,
@ -134,8 +134,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.vbatPidCompensation = 0, .vbatPidCompensation = 0,
.pidAtMinThrottle = PID_STABILISATION_ON, .pidAtMinThrottle = PID_STABILISATION_ON,
.levelAngleLimit = 55, .levelAngleLimit = 55,
.setpointRelaxRatio = 0, .feedForwardTransition = 0,
.dtermSetpointWeight = 60,
.yawRateAccelLimit = 100, .yawRateAccelLimit = 100,
.rateAccelLimit = 0, .rateAccelLimit = 0,
.itermThrottleThreshold = 250, .itermThrottleThreshold = 250,
@ -209,11 +208,11 @@ typedef union dtermLowpass_u {
} dtermLowpass_t; } dtermLowpass_t;
static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermNotchApplyFn; static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermNotchApplyFn;
static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch[2]; static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpassApplyFn; static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpassApplyFn;
static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass[2]; static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn; static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn;
static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2[2]; static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn; static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn;
static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass; static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass;
#if defined(USE_ITERM_RELAX) #if defined(USE_ITERM_RELAX)
@ -224,8 +223,8 @@ static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoff;
#endif #endif
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1[2]; static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT biquadFilter_t setpointDerivativeBiquad[2]; static FAST_RAM_ZERO_INIT biquadFilter_t setpointDerivativeBiquad[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT bool setpointDerivativeLpfInitialized; static FAST_RAM_ZERO_INIT bool setpointDerivativeLpfInitialized;
static FAST_RAM_ZERO_INIT uint8_t rcSmoothingDebugAxis; static FAST_RAM_ZERO_INIT uint8_t rcSmoothingDebugAxis;
static FAST_RAM_ZERO_INIT uint8_t rcSmoothingFilterType; static FAST_RAM_ZERO_INIT uint8_t rcSmoothingFilterType;
@ -235,7 +234,7 @@ static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf;
void pidInitFilters(const pidProfile_t *pidProfile) void pidInitFilters(const pidProfile_t *pidProfile)
{ {
BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2 BUILD_BUG_ON(FD_YAW != 2); // ensure yaw axis is 2
if (targetPidLooptime == 0) { if (targetPidLooptime == 0) {
// no looptime set, so set all the filters to null // no looptime set, so set all the filters to null
@ -261,7 +260,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
if (dTermNotchHz != 0 && pidProfile->dterm_notch_cutoff != 0) { if (dTermNotchHz != 0 && pidProfile->dterm_notch_cutoff != 0) {
dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply; dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff); const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff);
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInit(&dtermNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH); biquadFilterInit(&dtermNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH);
} }
} else { } else {
@ -273,7 +272,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
dtermLowpass2ApplyFn = nullFilterApply; dtermLowpass2ApplyFn = nullFilterApply;
} else { } else {
dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply; dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt1FilterInit(&dtermLowpass2[axis], pt1FilterGain(pidProfile->dterm_lowpass2_hz, dT)); pt1FilterInit(&dtermLowpass2[axis], pt1FilterGain(pidProfile->dterm_lowpass2_hz, dT));
} }
} }
@ -287,13 +286,13 @@ void pidInitFilters(const pidProfile_t *pidProfile)
break; break;
case FILTER_PT1: case FILTER_PT1:
dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply; dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt1FilterInit(&dtermLowpass[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lowpass_hz, dT)); pt1FilterInit(&dtermLowpass[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lowpass_hz, dT));
} }
break; break;
case FILTER_BIQUAD: case FILTER_BIQUAD:
dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply; dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime); biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime);
} }
break; break;
@ -328,7 +327,7 @@ void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint
rcSmoothingFilterType = filterType; rcSmoothingFilterType = filterType;
if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) { if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
setpointDerivativeLpfInitialized = true; setpointDerivativeLpfInitialized = true;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
switch (rcSmoothingFilterType) { switch (rcSmoothingFilterType) {
case RC_SMOOTHING_DERIVATIVE_PT1: case RC_SMOOTHING_DERIVATIVE_PT1:
pt1FilterInit(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT)); pt1FilterInit(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
@ -344,7 +343,7 @@ void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff) void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff)
{ {
if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) { if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
switch (rcSmoothingFilterType) { switch (rcSmoothingFilterType) {
case RC_SMOOTHING_DERIVATIVE_PT1: case RC_SMOOTHING_DERIVATIVE_PT1:
pt1FilterUpdateCutoff(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT)); pt1FilterUpdateCutoff(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
@ -362,12 +361,12 @@ typedef struct pidCoefficient_s {
float Kp; float Kp;
float Ki; float Ki;
float Kd; float Kd;
float Kf;
} pidCoefficient_t; } pidCoefficient_t;
static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient[3]; static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float maxVelocity[3]; static FAST_RAM_ZERO_INIT float maxVelocity[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float relaxFactor; static FAST_RAM_ZERO_INIT float feedForwardTransition;
static FAST_RAM_ZERO_INIT float dtermSetpointWeight;
static FAST_RAM_ZERO_INIT float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio; static FAST_RAM_ZERO_INIT float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio;
static FAST_RAM_ZERO_INIT float ITermWindupPointInv; static FAST_RAM_ZERO_INIT float ITermWindupPointInv;
static FAST_RAM_ZERO_INIT uint8_t horizonTiltExpertMode; static FAST_RAM_ZERO_INIT uint8_t horizonTiltExpertMode;
@ -424,18 +423,18 @@ void pidUpdateAntiGravityThrottleFilter(float throttle)
void pidInitConfig(const pidProfile_t *pidProfile) void pidInitConfig(const pidProfile_t *pidProfile)
{ {
if (pidProfile->feedForwardTransition == 0) {
feedForwardTransition = 0;
} else {
feedForwardTransition = 100.0f / pidProfile->feedForwardTransition;
}
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P; pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P;
pidCoefficient[axis].Ki = ITERM_SCALE * pidProfile->pid[axis].I; pidCoefficient[axis].Ki = ITERM_SCALE * pidProfile->pid[axis].I;
pidCoefficient[axis].Kd = DTERM_SCALE * pidProfile->pid[axis].D; pidCoefficient[axis].Kd = DTERM_SCALE * pidProfile->pid[axis].D;
pidCoefficient[axis].Kf = FEEDFORWARD_SCALE * (pidProfile->pid[axis].F / 100.0f);
} }
dtermSetpointWeight = pidProfile->dtermSetpointWeight / 100.0f;
if (pidProfile->setpointRelaxRatio == 0) {
relaxFactor = 0;
} else {
relaxFactor = 100.0f / pidProfile->setpointRelaxRatio;
}
levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f; levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f;
horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f; horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f;
horizonTransition = (float)pidProfile->pid[PID_LEVEL].D; horizonTransition = (float)pidProfile->pid[PID_LEVEL].D;
@ -598,7 +597,7 @@ static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPit
static float accelerationLimit(int axis, float currentPidSetpoint) static float accelerationLimit(int axis, float currentPidSetpoint)
{ {
static float previousSetpoint[3]; static float previousSetpoint[XYZ_AXIS_COUNT];
const float currentVelocity = currentPidSetpoint - previousSetpoint[axis]; const float currentVelocity = currentPidSetpoint - previousSetpoint[axis];
if (ABS(currentVelocity) > maxVelocity[axis]) { if (ABS(currentVelocity) > maxVelocity[axis]) {
@ -701,7 +700,7 @@ static void rotateITermAndAxisError()
#endif #endif
) { ) {
const float gyroToAngle = dT * RAD; const float gyroToAngle = dT * RAD;
float rotationRads[3]; float rotationRads[XYZ_AXIS_COUNT];
for (int i = FD_ROLL; i <= FD_YAW; i++) { for (int i = FD_ROLL; i <= FD_YAW; i++) {
rotationRads[i] = gyro.gyroADCf[i] * gyroToAngle; rotationRads[i] = gyro.gyroADCf[i] * gyroToAngle;
} }
@ -711,7 +710,7 @@ static void rotateITermAndAxisError()
} }
#endif #endif
if (itermRotation) { if (itermRotation) {
float v[3]; float v[XYZ_AXIS_COUNT];
for (int i = 0; i < XYZ_AXIS_COUNT; i++) { for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
v[i] = pidData[i].I; v[i] = pidData[i].I;
} }
@ -797,12 +796,51 @@ static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTri
} }
#endif // USE_ACRO_TRAINER #endif // USE_ACRO_TRAINER
#ifdef USE_RC_SMOOTHING_FILTER
float FAST_CODE applyRcSmoothingDerivativeFilter(int axis, float pidSetpointDelta)
{
float ret = pidSetpointDelta;
if (axis == rcSmoothingDebugAxis) {
DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f));
}
if (setpointDerivativeLpfInitialized) {
switch (rcSmoothingFilterType) {
case RC_SMOOTHING_DERIVATIVE_PT1:
ret = pt1FilterApply(&setpointDerivativePt1[axis], pidSetpointDelta);
break;
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
ret = biquadFilterApplyDF1(&setpointDerivativeBiquad[axis], pidSetpointDelta);
break;
}
if (axis == rcSmoothingDebugAxis) {
DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(ret * 100.0f));
}
}
return ret;
}
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_SMART_FEEDFORWARD
void FAST_CODE applySmartFeedforward(int axis)
{
if (smartFeedforward) {
if (pidData[axis].P * pidData[axis].F > 0) {
if (ABS(pidData[axis].F) > ABS(pidData[axis].P)) {
pidData[axis].P = 0;
} else {
pidData[axis].F = 0;
}
}
}
}
#endif // USE_SMART_FEEDFORWARD
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
// Based on 2DOF reference design (matlab) // Based on 2DOF reference design (matlab)
void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs) void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs)
{ {
static float previousGyroRateDterm[2]; static float previousGyroRateDterm[XYZ_AXIS_COUNT];
static float previousPidSetpoint[2]; static float previousPidSetpoint[XYZ_AXIS_COUNT];
const float tpaFactor = getThrottlePIDAttenuation(); const float tpaFactor = getThrottlePIDAttenuation();
const float motorMixRange = getMotorMixRange(); const float motorMixRange = getMotorMixRange();
@ -820,12 +858,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
// gradually scale back integration when above windup point // gradually scale back integration when above windup point
const float dynCi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f) * dT * itermAccelerator; const float dynCi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f) * dT * itermAccelerator;
// Dynamic d component, enable 2-DOF PID controller only for rate mode
const float dynCd = flightModeFlags ? 0.0f : dtermSetpointWeight;
// Precalculate gyro deta for D-term here, this allows loop unrolling // Precalculate gyro deta for D-term here, this allows loop unrolling
float gyroRateDterm[2]; float gyroRateDterm[XYZ_AXIS_COUNT];
for (int axis = FD_ROLL; axis < FD_YAW; ++axis) { for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
gyroRateDterm[axis] = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyro.gyroADCf[axis]); gyroRateDterm[axis] = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyro.gyroADCf[axis]);
gyroRateDterm[axis] = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateDterm[axis]); gyroRateDterm[axis] = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateDterm[axis]);
gyroRateDterm[axis] = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateDterm[axis]); gyroRateDterm[axis] = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateDterm[axis]);
@ -835,12 +870,13 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
// ----------PID controller---------- // ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) { for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
float currentPidSetpoint = getSetpointRate(axis); float currentPidSetpoint = getSetpointRate(axis);
if (maxVelocity[axis]) { if (maxVelocity[axis]) {
currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint); currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
} }
// Yaw control is GYRO based, direct sticks control is applied to rate PID // Yaw control is GYRO based, direct sticks control is applied to rate PID
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) && axis != YAW) { if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) && axis != FD_YAW) {
currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint); currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
} }
@ -937,7 +973,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
// --------low-level gyro-based PID based on 2DOF PID controller. ---------- // --------low-level gyro-based PID based on 2DOF PID controller. ----------
// 2-DOF PID controller with optional filter on derivative term. // 2-DOF PID controller with optional filter on derivative term.
// b = 1 and only c (dtermSetpointWeight) can be tuned (amount derivative on measurement or error). // b = 1 and only c (feedforward weight) can be tuned (amount derivative on measurement or error).
// -----calculate P component and add Dynamic Part based on stick input // -----calculate P component and add Dynamic Part based on stick input
pidData[axis].P = pidCoefficient[axis].Kp * errorRate * tpaFactor; pidData[axis].P = pidCoefficient[axis].Kp * errorRate * tpaFactor;
@ -954,9 +990,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
} }
// -----calculate D component // -----calculate D component
if (axis != FD_YAW) { if (pidCoefficient[axis].Kd > 0) {
// no transition if relaxFactor == 0
float transition = relaxFactor > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * relaxFactor) : 1;
// Divide rate change by dT to get differential (ie dr/dt). // Divide rate change by dT to get differential (ie dr/dt).
// dT is fixed and calculated from the target PID loop time // dT is fixed and calculated from the target PID loop time
@ -969,73 +1003,52 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate); detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate);
pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor; pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor;
} else {
pidData[axis].D = 0;
}
previousGyroRateDterm[axis] = gyroRateDterm[axis];
// -----calculate feedforward component
// Only enable feedforward for rate mode
const float feedforwardGain = flightModeFlags ? 0.0f : pidCoefficient[axis].Kf;
if (feedforwardGain > 0) {
// no transition if feedForwardTransition == 0
float transition = feedForwardTransition > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * feedForwardTransition) : 1;
float pidSetpointDelta = currentPidSetpoint - previousPidSetpoint[axis]; float pidSetpointDelta = currentPidSetpoint - previousPidSetpoint[axis];
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
if (axis == rcSmoothingDebugAxis) { pidSetpointDelta = applyRcSmoothingDerivativeFilter(axis, pidSetpointDelta);
DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f));
}
if ((dynCd != 0) && setpointDerivativeLpfInitialized) {
switch (rcSmoothingFilterType) {
case RC_SMOOTHING_DERIVATIVE_PT1:
pidSetpointDelta = pt1FilterApply(&setpointDerivativePt1[axis], pidSetpointDelta);
break;
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
pidSetpointDelta = biquadFilterApplyDF1(&setpointDerivativeBiquad[axis], pidSetpointDelta);
break;
}
if (axis == rcSmoothingDebugAxis) {
DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(pidSetpointDelta * 100.0f));
}
}
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
const float pidFeedForward = pidData[axis].F = feedforwardGain * transition * pidSetpointDelta * pidFrequency;
pidCoefficient[axis].Kd * dynCd * transition * pidSetpointDelta * tpaFactor * pidFrequency;
#if defined(USE_SMART_FEEDFORWARD) #if defined(USE_SMART_FEEDFORWARD)
bool addFeedforward = true; applySmartFeedforward(axis);
if (smartFeedforward) {
if (pidData[axis].P * pidFeedForward > 0) {
if (ABS(pidFeedForward) > ABS(pidData[axis].P)) {
pidData[axis].P = 0;
} else {
addFeedforward = false;
}
}
}
if (addFeedforward)
#endif #endif
{ } else {
pidData[axis].D += pidFeedForward; pidData[axis].F = 0;
} }
previousGyroRateDterm[axis] = gyroRateDterm[axis]; previousPidSetpoint[axis] = currentPidSetpoint;
previousPidSetpoint[axis] = currentPidSetpoint;
#ifdef USE_YAW_SPIN_RECOVERY #ifdef USE_YAW_SPIN_RECOVERY
if (yawSpinActive) { if (yawSpinActive) {
pidData[axis].I = 0; // in yaw spin always disable I
if (axis <= FD_PITCH) {
// zero PIDs on pitch and roll leaving yaw P to correct spin // zero PIDs on pitch and roll leaving yaw P to correct spin
pidData[axis].P = 0; pidData[axis].P = 0;
pidData[axis].I = 0;
pidData[axis].D = 0; pidData[axis].D = 0;
pidData[axis].F = 0;
} }
#endif // USE_YAW_SPIN_RECOVERY
} }
}
// calculating the PID sum
pidData[FD_ROLL].Sum = pidData[FD_ROLL].P + pidData[FD_ROLL].I + pidData[FD_ROLL].D;
pidData[FD_PITCH].Sum = pidData[FD_PITCH].P + pidData[FD_PITCH].I + pidData[FD_PITCH].D;
#ifdef USE_YAW_SPIN_RECOVERY
if (yawSpinActive) {
// yaw P alone to correct spin
pidData[FD_YAW].I = 0;
}
#endif // USE_YAW_SPIN_RECOVERY #endif // USE_YAW_SPIN_RECOVERY
// YAW has no D // calculating the PID sum
pidData[FD_YAW].Sum = pidData[FD_YAW].P + pidData[FD_YAW].I; pidData[axis].Sum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F;
}
// Disable PID control if at zero throttle or if gyro overflow detected // Disable PID control if at zero throttle or if gyro overflow detected
// This may look very innefficient, but it is done on purpose to always show real CPU usage as in flight // This may look very innefficient, but it is done on purpose to always show real CPU usage as in flight
@ -1044,6 +1057,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
pidData[axis].P = 0; pidData[axis].P = 0;
pidData[axis].I = 0; pidData[axis].I = 0;
pidData[axis].D = 0; pidData[axis].D = 0;
pidData[axis].F = 0;
pidData[axis].Sum = 0; pidData[axis].Sum = 0;
} }

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

@ -39,6 +39,10 @@
#define ITERM_SCALE 0.244381f #define ITERM_SCALE 0.244381f
#define DTERM_SCALE 0.000529f #define DTERM_SCALE 0.000529f
// The constant scale factor to replace the Kd component of the feedforward calculation.
// This value gives the same "feel" as the previous Kd default of 26 (26 * DTERM_SCALE)
#define FEEDFORWARD_SCALE 0.013754f
typedef enum { typedef enum {
PID_ROLL, PID_ROLL,
PID_PITCH, PID_PITCH,
@ -70,11 +74,12 @@ typedef enum {
PID_CRASH_RECOVERY_BEEP PID_CRASH_RECOVERY_BEEP
} pidCrashRecovery_e; } pidCrashRecovery_e;
typedef struct pid8_s { typedef struct pidf_s {
uint8_t P; uint8_t P;
uint8_t I; uint8_t I;
uint8_t D; uint8_t D;
} pid8_t; uint16_t F;
} pidf_t;
typedef enum { typedef enum {
ANTI_GRAVITY_SMOOTH, ANTI_GRAVITY_SMOOTH,
@ -100,7 +105,7 @@ typedef struct pidProfile_s {
uint16_t dterm_notch_hz; // Biquad dterm notch hz uint16_t dterm_notch_hz; // Biquad dterm notch hz
uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff
pid8_t pid[PID_ITEM_COUNT]; pidf_t pid[PID_ITEM_COUNT];
uint8_t dterm_filter_type; // Filter selection for dterm uint8_t dterm_filter_type; // Filter selection for dterm
uint8_t itermWindupPointPercent; // Experimental ITerm windup threshold, percent motor saturation uint8_t itermWindupPointPercent; // Experimental ITerm windup threshold, percent motor saturation
@ -116,7 +121,6 @@ typedef struct pidProfile_s {
uint8_t antiGravityMode; // type of anti gravity method uint8_t antiGravityMode; // type of anti gravity method
uint16_t itermThrottleThreshold; // max allowed throttle delta before iterm accelerated in ms uint16_t itermThrottleThreshold; // max allowed throttle delta before iterm accelerated in ms
uint16_t itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit uint16_t itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit
uint16_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > aggressive derivative)
uint16_t yawRateAccelLimit; // yaw accel limiter for deg/sec/ms uint16_t yawRateAccelLimit; // yaw accel limiter for deg/sec/ms
uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms
uint16_t crash_dthreshold; // dterm crash value uint16_t crash_dthreshold; // dterm crash value
@ -127,7 +131,7 @@ typedef struct pidProfile_s {
uint8_t crash_recovery_angle; // degrees uint8_t crash_recovery_angle; // degrees
uint8_t crash_recovery_rate; // degree/second uint8_t crash_recovery_rate; // degree/second
uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage
uint8_t setpointRelaxRatio; // Setpoint weight relaxation effect uint8_t feedForwardTransition; // Feed forward weight transition
uint16_t crash_limit_yaw; // limits yaw errorRate, so crashes don't cause huge throttle increase uint16_t crash_limit_yaw; // limits yaw errorRate, so crashes don't cause huge throttle increase
uint16_t itermLimit; uint16_t itermLimit;
uint16_t dterm_lowpass2_hz; // Extra PT1 Filter on D in hz uint16_t dterm_lowpass2_hz; // Extra PT1 Filter on D in hz
@ -168,6 +172,7 @@ typedef struct pidAxisData_s {
float P; float P;
float I; float I;
float D; float D;
float F;
float Sum; float Sum;
} pidAxisData_t; } pidAxisData_t;
@ -195,4 +200,4 @@ void pidUpdateAntiGravityThrottleFilter(float throttle);
bool pidOsdAntiGravityActive(void); bool pidOsdAntiGravityActive(void);
bool pidOsdAntiGravityMode(void); bool pidOsdAntiGravityMode(void);
void pidSetAntiGravityState(bool newState); void pidSetAntiGravityState(bool newState);
bool pidAntiGravityEnabled(void); bool pidAntiGravityEnabled(void);

21
src/main/interface/msp.c
View file

@ -1293,8 +1293,8 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, 0); // was pidProfile.yaw_p_limit sbufWriteU16(dst, 0); // was pidProfile.yaw_p_limit
sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved
sbufWriteU8(dst, currentPidProfile->vbatPidCompensation); sbufWriteU8(dst, currentPidProfile->vbatPidCompensation);
sbufWriteU8(dst, currentPidProfile->setpointRelaxRatio); sbufWriteU8(dst, currentPidProfile->feedForwardTransition);
sbufWriteU8(dst, MIN(currentPidProfile->dtermSetpointWeight, 255)); sbufWriteU8(dst, 0); // was low byte of currentPidProfile->dtermSetpointWeight
sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved
sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved
sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved
@ -1304,7 +1304,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, 0); // was pidProfile.levelSensitivity sbufWriteU8(dst, 0); // was pidProfile.levelSensitivity
sbufWriteU16(dst, currentPidProfile->itermThrottleThreshold); sbufWriteU16(dst, currentPidProfile->itermThrottleThreshold);
sbufWriteU16(dst, currentPidProfile->itermAcceleratorGain); sbufWriteU16(dst, currentPidProfile->itermAcceleratorGain);
sbufWriteU16(dst, currentPidProfile->dtermSetpointWeight); sbufWriteU16(dst, 0); // was currentPidProfile->dtermSetpointWeight
sbufWriteU8(dst, currentPidProfile->iterm_rotation); sbufWriteU8(dst, currentPidProfile->iterm_rotation);
#if defined(USE_SMART_FEEDFORWARD) #if defined(USE_SMART_FEEDFORWARD)
sbufWriteU8(dst, currentPidProfile->smart_feedforward); sbufWriteU8(dst, currentPidProfile->smart_feedforward);
@ -1333,6 +1333,9 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
#else #else
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
#endif #endif
sbufWriteU16(dst, currentPidProfile->pid[PID_ROLL].F);
sbufWriteU16(dst, currentPidProfile->pid[PID_PITCH].F);
sbufWriteU16(dst, currentPidProfile->pid[PID_YAW].F);
break; break;
case MSP_SENSOR_CONFIG: case MSP_SENSOR_CONFIG:
@ -1835,8 +1838,8 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
sbufReadU16(src); // was pidProfile.yaw_p_limit sbufReadU16(src); // was pidProfile.yaw_p_limit
sbufReadU8(src); // reserved sbufReadU8(src); // reserved
currentPidProfile->vbatPidCompensation = sbufReadU8(src); currentPidProfile->vbatPidCompensation = sbufReadU8(src);
currentPidProfile->setpointRelaxRatio = sbufReadU8(src); currentPidProfile->feedForwardTransition = sbufReadU8(src);
currentPidProfile->dtermSetpointWeight = sbufReadU8(src); sbufReadU8(src); // was low byte of currentPidProfile->dtermSetpointWeight
sbufReadU8(src); // reserved sbufReadU8(src); // reserved
sbufReadU8(src); // reserved sbufReadU8(src); // reserved
sbufReadU8(src); // reserved sbufReadU8(src); // reserved
@ -1851,9 +1854,9 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
currentPidProfile->itermAcceleratorGain = sbufReadU16(src); currentPidProfile->itermAcceleratorGain = sbufReadU16(src);
} }
if (sbufBytesRemaining(src) >= 2) { if (sbufBytesRemaining(src) >= 2) {
currentPidProfile->dtermSetpointWeight = sbufReadU16(src); sbufReadU16(src); // was currentPidProfile->dtermSetpointWeight
} }
if (sbufBytesRemaining(src) >= 7) { if (sbufBytesRemaining(src) >= 13) {
// Added in MSP API 1.40 // Added in MSP API 1.40
currentPidProfile->iterm_rotation = sbufReadU8(src); currentPidProfile->iterm_rotation = sbufReadU8(src);
#if defined(USE_SMART_FEEDFORWARD) #if defined(USE_SMART_FEEDFORWARD)
@ -1883,6 +1886,10 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#else #else
sbufReadU8(src); sbufReadU8(src);
#endif #endif
// PID controller feedforward terms
currentPidProfile->pid[PID_ROLL].F = sbufReadU16(src);
currentPidProfile->pid[PID_PITCH].F = sbufReadU16(src);
currentPidProfile->pid[PID_YAW].F = sbufReadU16(src);
} }
pidInitConfig(currentPidProfile); pidInitConfig(currentPidProfile);

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

@ -792,8 +792,7 @@ const clivalue_t valueTable[] = {
{ "anti_gravity_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ANTI_GRAVITY_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, antiGravityMode) }, { "anti_gravity_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ANTI_GRAVITY_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, antiGravityMode) },
{ "anti_gravity_threshold", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 20, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermThrottleThreshold) }, { "anti_gravity_threshold", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 20, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermThrottleThreshold) },
{ "anti_gravity_gain", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1000, 30000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermAcceleratorGain) }, { "anti_gravity_gain", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1000, 30000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermAcceleratorGain) },
{ "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, setpointRelaxRatio) }, { "feedforward_transition", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedForwardTransition) },
{ "dterm_setpoint_weight", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dtermSetpointWeight) },
{ "acc_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawRateAccelLimit) }, { "acc_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawRateAccelLimit) },
{ "acc_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, rateAccelLimit) }, { "acc_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, rateAccelLimit) },
{ "crash_dthreshold", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_dthreshold) }, { "crash_dthreshold", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_dthreshold) },
@ -836,12 +835,15 @@ const clivalue_t valueTable[] = {
{ "p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].P) }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].P) },
{ "i_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].I) }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].I) },
{ "d_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].D) }, { "d_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].D) },
{ "f_pitch", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 },PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].F) },
{ "p_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ROLL].P) }, { "p_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ROLL].P) },
{ "i_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ROLL].I) }, { "i_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ROLL].I) },
{ "d_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ROLL].D) }, { "d_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ROLL].D) },
{ "f_roll", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 },PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ROLL].F) },
{ "p_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].P) }, { "p_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].P) },
{ "i_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].I) }, { "i_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].I) },
{ "d_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].D) }, { "d_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].D) },
{ "f_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 },PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].F) },
{ "p_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].P) }, { "p_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].P) },
{ "i_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].I) }, { "i_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].I) },

2
src/main/io/osd.c
View file

@ -282,7 +282,7 @@ static void osdFormatAltitudeString(char * buff, int altitude)
buff[4] = '.'; buff[4] = '.';
} }
static void osdFormatPID(char * buff, const char * label, const pid8_t * pid) static void osdFormatPID(char * buff, const char * label, const pidf_t * pid)
{ {
tfp_sprintf(buff, "%s %3d %3d %3d", label, pid->P, pid->I, pid->D); tfp_sprintf(buff, "%s %3d %3d %3d", label, pid->P, pid->I, pid->D);
} }

2
src/main/pg/rx.c
View file

@ -56,7 +56,7 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
.rssi_offset = 0, .rssi_offset = 0,
.rssi_invert = 0, .rssi_invert = 0,
.rcInterpolation = RC_SMOOTHING_AUTO, .rcInterpolation = RC_SMOOTHING_AUTO,
.rcInterpolationChannels = INTERPOLATION_CHANNELS_RPT, .rcInterpolationChannels = INTERPOLATION_CHANNELS_RPYT,
.rcInterpolationInterval = 19, .rcInterpolationInterval = 19,
.fpvCamAngleDegrees = 0, .fpvCamAngleDegrees = 0,
.airModeActivateThreshold = 32, .airModeActivateThreshold = 32,

5
src/main/target/ALIENWHOOP/config.c
View file

@ -139,8 +139,9 @@ void targetConfiguration(void)
/* Setpoints */ /* Setpoints */
pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_filter_type = FILTER_BIQUAD;
pidProfile->dterm_notch_hz = 0; pidProfile->dterm_notch_hz = 0;
pidProfile->dtermSetpointWeight = 100; pidProfile->pid[PID_PITCH].F = 100;
pidProfile->setpointRelaxRatio = 0; pidProfile->pid[PID_ROLL].F = 100;
pidProfile->feedForwardTransition = 0;
/* Anti-Gravity */ /* Anti-Gravity */
pidProfile->itermThrottleThreshold = 500; pidProfile->itermThrottleThreshold = 500;

5
src/main/target/NAZE/config.c
View file

@ -85,8 +85,9 @@ void targetConfiguration(void)
pidProfile->pid[PID_LEVEL].P = 30; pidProfile->pid[PID_LEVEL].P = 30;
pidProfile->pid[PID_LEVEL].D = 30; pidProfile->pid[PID_LEVEL].D = 30;
pidProfile->dtermSetpointWeight = 200; pidProfile->pid[PID_PITCH].F = 200;
pidProfile->setpointRelaxRatio = 50; pidProfile->pid[PID_ROLL].F = 200;
pidProfile->feedForwardTransition = 50;
} }
for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) { for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) {

13
src/test/unit/pid_unittest.cc
View file

@ -84,10 +84,10 @@ int loopIter = 0;
void setDefaultTestSettings(void) { void setDefaultTestSettings(void) {
pgResetAll(); pgResetAll();
pidProfile = pidProfilesMutable(1); pidProfile = pidProfilesMutable(1);
pidProfile->pid[PID_ROLL] = { 40, 40, 30 }; pidProfile->pid[PID_ROLL] = { 40, 40, 30, 65 };
pidProfile->pid[PID_PITCH] = { 58, 50, 35 }; pidProfile->pid[PID_PITCH] = { 58, 50, 35, 60 };
pidProfile->pid[PID_YAW] = { 70, 45, 0 }; pidProfile->pid[PID_YAW] = { 70, 45, 20, 60 };
pidProfile->pid[PID_LEVEL] = { 50, 50, 75 }; pidProfile->pid[PID_LEVEL] = { 50, 50, 75, 0 };
pidProfile->pidSumLimit = PIDSUM_LIMIT; pidProfile->pidSumLimit = PIDSUM_LIMIT;
pidProfile->pidSumLimitYaw = PIDSUM_LIMIT_YAW; pidProfile->pidSumLimitYaw = PIDSUM_LIMIT_YAW;
@ -101,8 +101,7 @@ void setDefaultTestSettings(void) {
pidProfile->vbatPidCompensation = 0; pidProfile->vbatPidCompensation = 0;
pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
pidProfile->levelAngleLimit = 55; pidProfile->levelAngleLimit = 55;
pidProfile->setpointRelaxRatio = 100; pidProfile->feedForwardTransition = 100;
pidProfile->dtermSetpointWeight = 0;
pidProfile->yawRateAccelLimit = 100; pidProfile->yawRateAccelLimit = 100;
pidProfile->rateAccelLimit = 0; pidProfile->rateAccelLimit = 0;
pidProfile->antiGravityMode = ANTI_GRAVITY_SMOOTH; pidProfile->antiGravityMode = ANTI_GRAVITY_SMOOTH;
@ -267,7 +266,7 @@ TEST(pidControllerTest, testPidLoop) {
ASSERT_NEAR(-8.7, pidData[FD_YAW].I, calculateTolerance(-8.7)); ASSERT_NEAR(-8.7, pidData[FD_YAW].I, calculateTolerance(-8.7));
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D); EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D); EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D); EXPECT_FLOAT_EQ(-132.25, pidData[FD_YAW].D);
// Match the stick to gyro to stop error // Match the stick to gyro to stop error
simulatedSetpointRate[FD_ROLL] = 100; simulatedSetpointRate[FD_ROLL] = 100;