mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-17 05:15:25 +03:00
PID controller feedforward
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
parent
625b23915e
commit
17e76e48f6
13 changed files with 201 additions and 145 deletions
|
|
@ -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", 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)},
|
||||
{"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: */
|
||||
{"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)},
|
||||
|
|
@ -285,6 +288,7 @@ typedef struct blackboxMainState_s {
|
|||
int32_t axisPID_P[XYZ_AXIS_COUNT];
|
||||
int32_t axisPID_I[XYZ_AXIS_COUNT];
|
||||
int32_t axisPID_D[XYZ_AXIS_COUNT];
|
||||
int32_t axisPID_F[XYZ_AXIS_COUNT];
|
||||
|
||||
int16_t rcCommand[4];
|
||||
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:
|
||||
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
|
||||
* 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_I[i] = pidData[i].I;
|
||||
blackboxCurrent->axisPID_D[i] = pidData[i].D;
|
||||
blackboxCurrent->axisPID_F[i] = pidData[i].F;
|
||||
blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
|
||||
blackboxCurrent->accADC[i] = lrintf(acc.accADC[i]);
|
||||
#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_threshold", "%d", currentPidProfile->itermThrottleThreshold);
|
||||
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", "%d", currentPidProfile->rateAccelLimit);
|
||||
BLACKBOX_PRINT_HEADER_LINE("pidsum_limit", "%d", currentPidProfile->pidSumLimit);
|
||||
|
|
|
|||
|
|
@ -58,6 +58,7 @@ static uint8_t tmpPidProfileIndex;
|
|||
static uint8_t pidProfileIndex;
|
||||
static char pidProfileIndexString[] = " p";
|
||||
static uint8_t tempPid[3][3];
|
||||
static uint16_t tempPidF[3];
|
||||
|
||||
static uint8_t tmpRateProfileIndex;
|
||||
static uint8_t rateProfileIndex;
|
||||
|
|
@ -115,6 +116,7 @@ static long cmsx_PidRead(void)
|
|||
tempPid[i][0] = pidProfile->pid[i].P;
|
||||
tempPid[i][1] = pidProfile->pid[i].I;
|
||||
tempPid[i][2] = pidProfile->pid[i].D;
|
||||
tempPidF[i] = pidProfile->pid[i].F;
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
|
@ -137,6 +139,7 @@ static long cmsx_PidWriteback(const OSD_Entry *self)
|
|||
pidProfile->pid[i].P = tempPid[i][0];
|
||||
pidProfile->pid[i].I = tempPid[i][1];
|
||||
pidProfile->pid[i].D = tempPid[i][2];
|
||||
pidProfile->pid[i].F = tempPidF[i];
|
||||
}
|
||||
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 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 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 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 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 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 F", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidF[PID_YAW], 0, 2000, 1 }, 0 },
|
||||
|
||||
{ "BACK", OME_Back, NULL, NULL, 0 },
|
||||
{ NULL, OME_END, NULL, NULL, 0 }
|
||||
|
|
@ -237,8 +243,7 @@ static CMS_Menu cmsx_menuRateProfile = {
|
|||
.entries = cmsx_menuRateProfileEntries
|
||||
};
|
||||
|
||||
static uint16_t cmsx_dtermSetpointWeight;
|
||||
static uint8_t cmsx_setpointRelaxRatio;
|
||||
static uint8_t cmsx_feedForwardTransition;
|
||||
static uint8_t cmsx_angleStrength;
|
||||
static uint8_t cmsx_horizonStrength;
|
||||
static uint8_t cmsx_horizonTransition;
|
||||
|
|
@ -250,8 +255,8 @@ static long cmsx_profileOtherOnEnter(void)
|
|||
pidProfileIndexString[1] = '0' + tmpPidProfileIndex;
|
||||
|
||||
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_horizonStrength = pidProfile->pid[PID_LEVEL].I;
|
||||
|
|
@ -268,8 +273,7 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self)
|
|||
UNUSED(self);
|
||||
|
||||
pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
|
||||
pidProfile->dtermSetpointWeight = cmsx_dtermSetpointWeight;
|
||||
pidProfile->setpointRelaxRatio = cmsx_setpointRelaxRatio;
|
||||
pidProfile->feedForwardTransition = cmsx_feedForwardTransition;
|
||||
pidInitConfig(currentPidProfile);
|
||||
|
||||
pidProfile->pid[PID_LEVEL].P = cmsx_angleStrength;
|
||||
|
|
@ -285,8 +289,7 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self)
|
|||
static OSD_Entry cmsx_menuProfileOtherEntries[] = {
|
||||
{ "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString, 0 },
|
||||
|
||||
{ "D SETPT WT", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_dtermSetpointWeight, 0, 2000, 1 }, 0 },
|
||||
{ "SETPT TRS", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_setpointRelaxRatio, 0, 100, 1, 10 }, 0 },
|
||||
{ "FF TRANS", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_feedForwardTransition, 0, 100, 1, 10 }, 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 TRS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonTransition, 0, 200, 1 } , 0 },
|
||||
|
|
|
|||
|
|
@ -263,10 +263,20 @@ static void validateAndFixConfig(void)
|
|||
|
||||
if (!rcSmoothingIsEnabled() || rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_T) {
|
||||
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 (!rcSmoothingIsEnabled() ||
|
||||
!(rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_RPYT
|
||||
|
|
|
|||
|
|
@ -406,13 +406,13 @@ static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t a
|
|||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue);
|
||||
break;
|
||||
case ADJUSTMENT_D_SETPOINT:
|
||||
newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 2000); // FIXME magic numbers repeated in cli.c
|
||||
pidProfile->dtermSetpointWeight = newValue;
|
||||
newValue = constrain((int)pidProfile->pid[PID_ROLL].F + delta, 0, 2000); // FIXME magic numbers repeated in cli.c
|
||||
pidProfile->pid[PID_ROLL].F = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue);
|
||||
break;
|
||||
case ADJUSTMENT_D_SETPOINT_TRANSITION:
|
||||
newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 1, 100); // FIXME magic numbers repeated in cli.c
|
||||
pidProfile->setpointRelaxRatio = newValue;
|
||||
newValue = constrain((int)pidProfile->feedForwardTransition + delta, 1, 100); // FIXME magic numbers repeated in cli.c
|
||||
pidProfile->feedForwardTransition = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue);
|
||||
break;
|
||||
default:
|
||||
|
|
@ -556,12 +556,12 @@ static int applyAbsoluteAdjustment(controlRateConfig_t *controlRateConfig, adjus
|
|||
break;
|
||||
case ADJUSTMENT_D_SETPOINT:
|
||||
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);
|
||||
break;
|
||||
case ADJUSTMENT_D_SETPOINT_TRANSITION:
|
||||
newValue = constrain(value, 1, 100); // FIXME magic numbers repeated in cli.c
|
||||
pidProfile->setpointRelaxRatio = newValue;
|
||||
pidProfile->feedForwardTransition = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue);
|
||||
break;
|
||||
default:
|
||||
|
|
|
|||
|
|
@ -104,22 +104,22 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
|
|||
|
||||
#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)
|
||||
{
|
||||
RESET_CONFIG(pidProfile_t, pidProfile,
|
||||
.pid = {
|
||||
[PID_ROLL] = { 46, 45, 25 },
|
||||
[PID_PITCH] = { 50, 50, 27 },
|
||||
[PID_YAW] = { 65, 45, 0 },
|
||||
[PID_ALT] = { 50, 0, 0 },
|
||||
[PID_POS] = { 15, 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_NAVR] = { 25, 33, 83 }, // NAV_P * 10, NAV_I * 100, NAV_D * 1000
|
||||
[PID_LEVEL] = { 50, 50, 75 },
|
||||
[PID_MAG] = { 40, 0, 0 },
|
||||
[PID_VEL] = { 55, 55, 75 }
|
||||
[PID_ROLL] = { 46, 45, 25, 60 },
|
||||
[PID_PITCH] = { 50, 50, 27, 60 },
|
||||
[PID_YAW] = { 65, 45, 0 , 60 },
|
||||
[PID_ALT] = { 50, 0, 0, 0 },
|
||||
[PID_POS] = { 15, 0, 0, 0 }, // POSHOLD_P * 100, POSHOLD_I * 100,
|
||||
[PID_POSR] = { 34, 14, 53, 0 }, // POSHOLD_RATE_P * 10, POSHOLD_RATE_I * 100, POSHOLD_RATE_D * 1000,
|
||||
[PID_NAVR] = { 25, 33, 83, 0 }, // NAV_P * 10, NAV_I * 100, NAV_D * 1000
|
||||
[PID_LEVEL] = { 50, 50, 75, 0 },
|
||||
[PID_MAG] = { 40, 0, 0, 0 },
|
||||
[PID_VEL] = { 55, 55, 75, 0 }
|
||||
},
|
||||
|
||||
.pidSumLimit = PIDSUM_LIMIT,
|
||||
|
|
@ -134,8 +134,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
|||
.vbatPidCompensation = 0,
|
||||
.pidAtMinThrottle = PID_STABILISATION_ON,
|
||||
.levelAngleLimit = 55,
|
||||
.setpointRelaxRatio = 0,
|
||||
.dtermSetpointWeight = 60,
|
||||
.feedForwardTransition = 0,
|
||||
.yawRateAccelLimit = 100,
|
||||
.rateAccelLimit = 0,
|
||||
.itermThrottleThreshold = 250,
|
||||
|
|
@ -209,11 +208,11 @@ typedef union dtermLowpass_u {
|
|||
} dtermLowpass_t;
|
||||
|
||||
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 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 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 pt1Filter_t ptermYawLowpass;
|
||||
#if defined(USE_ITERM_RELAX)
|
||||
|
|
@ -224,8 +223,8 @@ static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoff;
|
|||
#endif
|
||||
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1[2];
|
||||
static FAST_RAM_ZERO_INIT biquadFilter_t setpointDerivativeBiquad[2];
|
||||
static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1[XYZ_AXIS_COUNT];
|
||||
static FAST_RAM_ZERO_INIT biquadFilter_t setpointDerivativeBiquad[XYZ_AXIS_COUNT];
|
||||
static FAST_RAM_ZERO_INIT bool setpointDerivativeLpfInitialized;
|
||||
static FAST_RAM_ZERO_INIT uint8_t rcSmoothingDebugAxis;
|
||||
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)
|
||||
{
|
||||
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) {
|
||||
// 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) {
|
||||
dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||
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);
|
||||
}
|
||||
} else {
|
||||
|
|
@ -273,7 +272,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
|
|||
dtermLowpass2ApplyFn = nullFilterApply;
|
||||
} else {
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
|
@ -287,13 +286,13 @@ void pidInitFilters(const pidProfile_t *pidProfile)
|
|||
break;
|
||||
case FILTER_PT1:
|
||||
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));
|
||||
}
|
||||
break;
|
||||
case FILTER_BIQUAD:
|
||||
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);
|
||||
}
|
||||
break;
|
||||
|
|
@ -328,7 +327,7 @@ void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint
|
|||
rcSmoothingFilterType = filterType;
|
||||
if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
|
||||
setpointDerivativeLpfInitialized = true;
|
||||
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
||||
switch (rcSmoothingFilterType) {
|
||||
case RC_SMOOTHING_DERIVATIVE_PT1:
|
||||
pt1FilterInit(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
|
||||
|
|
@ -344,7 +343,7 @@ void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint
|
|||
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff)
|
||||
{
|
||||
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) {
|
||||
case RC_SMOOTHING_DERIVATIVE_PT1:
|
||||
pt1FilterUpdateCutoff(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
|
||||
|
|
@ -362,12 +361,12 @@ typedef struct pidCoefficient_s {
|
|||
float Kp;
|
||||
float Ki;
|
||||
float Kd;
|
||||
float Kf;
|
||||
} pidCoefficient_t;
|
||||
|
||||
static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient[3];
|
||||
static FAST_RAM_ZERO_INIT float maxVelocity[3];
|
||||
static FAST_RAM_ZERO_INIT float relaxFactor;
|
||||
static FAST_RAM_ZERO_INIT float dtermSetpointWeight;
|
||||
static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
|
||||
static FAST_RAM_ZERO_INIT float maxVelocity[XYZ_AXIS_COUNT];
|
||||
static FAST_RAM_ZERO_INIT float feedForwardTransition;
|
||||
static FAST_RAM_ZERO_INIT float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio;
|
||||
static FAST_RAM_ZERO_INIT float ITermWindupPointInv;
|
||||
static FAST_RAM_ZERO_INIT uint8_t horizonTiltExpertMode;
|
||||
|
|
@ -424,18 +423,18 @@ void pidUpdateAntiGravityThrottleFilter(float throttle)
|
|||
|
||||
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++) {
|
||||
pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P;
|
||||
pidCoefficient[axis].Ki = ITERM_SCALE * pidProfile->pid[axis].I;
|
||||
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;
|
||||
horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f;
|
||||
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 previousSetpoint[3];
|
||||
static float previousSetpoint[XYZ_AXIS_COUNT];
|
||||
const float currentVelocity = currentPidSetpoint - previousSetpoint[axis];
|
||||
|
||||
if (ABS(currentVelocity) > maxVelocity[axis]) {
|
||||
|
|
@ -701,7 +700,7 @@ static void rotateITermAndAxisError()
|
|||
#endif
|
||||
) {
|
||||
const float gyroToAngle = dT * RAD;
|
||||
float rotationRads[3];
|
||||
float rotationRads[XYZ_AXIS_COUNT];
|
||||
for (int i = FD_ROLL; i <= FD_YAW; i++) {
|
||||
rotationRads[i] = gyro.gyroADCf[i] * gyroToAngle;
|
||||
}
|
||||
|
|
@ -711,7 +710,7 @@ static void rotateITermAndAxisError()
|
|||
}
|
||||
#endif
|
||||
if (itermRotation) {
|
||||
float v[3];
|
||||
float v[XYZ_AXIS_COUNT];
|
||||
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
|
||||
v[i] = pidData[i].I;
|
||||
}
|
||||
|
|
@ -797,12 +796,51 @@ static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTri
|
|||
}
|
||||
#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.
|
||||
// Based on 2DOF reference design (matlab)
|
||||
void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs)
|
||||
{
|
||||
static float previousGyroRateDterm[2];
|
||||
static float previousPidSetpoint[2];
|
||||
static float previousGyroRateDterm[XYZ_AXIS_COUNT];
|
||||
static float previousPidSetpoint[XYZ_AXIS_COUNT];
|
||||
|
||||
const float tpaFactor = getThrottlePIDAttenuation();
|
||||
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
|
||||
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
|
||||
float gyroRateDterm[2];
|
||||
for (int axis = FD_ROLL; axis < FD_YAW; ++axis) {
|
||||
float gyroRateDterm[XYZ_AXIS_COUNT];
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
|
||||
gyroRateDterm[axis] = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyro.gyroADCf[axis]);
|
||||
gyroRateDterm[axis] = dtermLowpassApplyFn((filter_t *) &dtermLowpass[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----------
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
|
||||
|
||||
float currentPidSetpoint = getSetpointRate(axis);
|
||||
if (maxVelocity[axis]) {
|
||||
currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
|
||||
}
|
||||
// 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);
|
||||
}
|
||||
|
||||
|
|
@ -937,7 +973,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
|
|||
|
||||
// --------low-level gyro-based PID based on 2DOF PID controller. ----------
|
||||
// 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
|
||||
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
|
||||
if (axis != FD_YAW) {
|
||||
// no transition if relaxFactor == 0
|
||||
float transition = relaxFactor > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * relaxFactor) : 1;
|
||||
if (pidCoefficient[axis].Kd > 0) {
|
||||
|
||||
// Divide rate change by dT to get differential (ie dr/dt).
|
||||
// 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);
|
||||
|
||||
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];
|
||||
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
if (axis == rcSmoothingDebugAxis) {
|
||||
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));
|
||||
}
|
||||
}
|
||||
pidSetpointDelta = applyRcSmoothingDerivativeFilter(axis, pidSetpointDelta);
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
|
||||
const float pidFeedForward =
|
||||
pidCoefficient[axis].Kd * dynCd * transition * pidSetpointDelta * tpaFactor * pidFrequency;
|
||||
pidData[axis].F = feedforwardGain * transition * pidSetpointDelta * pidFrequency;
|
||||
|
||||
#if defined(USE_SMART_FEEDFORWARD)
|
||||
bool addFeedforward = true;
|
||||
if (smartFeedforward) {
|
||||
if (pidData[axis].P * pidFeedForward > 0) {
|
||||
if (ABS(pidFeedForward) > ABS(pidData[axis].P)) {
|
||||
pidData[axis].P = 0;
|
||||
} else {
|
||||
addFeedforward = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (addFeedforward)
|
||||
applySmartFeedforward(axis);
|
||||
#endif
|
||||
{
|
||||
pidData[axis].D += pidFeedForward;
|
||||
}
|
||||
previousGyroRateDterm[axis] = gyroRateDterm[axis];
|
||||
previousPidSetpoint[axis] = currentPidSetpoint;
|
||||
} else {
|
||||
pidData[axis].F = 0;
|
||||
}
|
||||
previousPidSetpoint[axis] = currentPidSetpoint;
|
||||
|
||||
#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
|
||||
pidData[axis].P = 0;
|
||||
pidData[axis].I = 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
|
||||
|
||||
// YAW has no D
|
||||
pidData[FD_YAW].Sum = pidData[FD_YAW].P + pidData[FD_YAW].I;
|
||||
// calculating the PID sum
|
||||
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
|
||||
// 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].I = 0;
|
||||
pidData[axis].D = 0;
|
||||
pidData[axis].F = 0;
|
||||
|
||||
pidData[axis].Sum = 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -39,6 +39,10 @@
|
|||
#define ITERM_SCALE 0.244381f
|
||||
#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 {
|
||||
PID_ROLL,
|
||||
PID_PITCH,
|
||||
|
|
@ -70,11 +74,12 @@ typedef enum {
|
|||
PID_CRASH_RECOVERY_BEEP
|
||||
} pidCrashRecovery_e;
|
||||
|
||||
typedef struct pid8_s {
|
||||
typedef struct pidf_s {
|
||||
uint8_t P;
|
||||
uint8_t I;
|
||||
uint8_t D;
|
||||
} pid8_t;
|
||||
uint16_t F;
|
||||
} pidf_t;
|
||||
|
||||
typedef enum {
|
||||
ANTI_GRAVITY_SMOOTH,
|
||||
|
|
@ -100,7 +105,7 @@ typedef struct pidProfile_s {
|
|||
uint16_t dterm_notch_hz; // Biquad dterm notch hz
|
||||
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 itermWindupPointPercent; // Experimental ITerm windup threshold, percent motor saturation
|
||||
|
|
@ -116,7 +121,6 @@ typedef struct pidProfile_s {
|
|||
uint8_t antiGravityMode; // type of anti gravity method
|
||||
uint16_t itermThrottleThreshold; // max allowed throttle delta before iterm accelerated in ms
|
||||
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 rateAccelLimit; // accel limiter roll/pitch deg/sec/ms
|
||||
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_rate; // degree/second
|
||||
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 itermLimit;
|
||||
uint16_t dterm_lowpass2_hz; // Extra PT1 Filter on D in hz
|
||||
|
|
@ -168,6 +172,7 @@ typedef struct pidAxisData_s {
|
|||
float P;
|
||||
float I;
|
||||
float D;
|
||||
float F;
|
||||
|
||||
float Sum;
|
||||
} pidAxisData_t;
|
||||
|
|
@ -195,4 +200,4 @@ void pidUpdateAntiGravityThrottleFilter(float throttle);
|
|||
bool pidOsdAntiGravityActive(void);
|
||||
bool pidOsdAntiGravityMode(void);
|
||||
void pidSetAntiGravityState(bool newState);
|
||||
bool pidAntiGravityEnabled(void);
|
||||
bool pidAntiGravityEnabled(void);
|
||||
|
|
|
|||
|
|
@ -1293,8 +1293,8 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
|
|||
sbufWriteU16(dst, 0); // was pidProfile.yaw_p_limit
|
||||
sbufWriteU8(dst, 0); // reserved
|
||||
sbufWriteU8(dst, currentPidProfile->vbatPidCompensation);
|
||||
sbufWriteU8(dst, currentPidProfile->setpointRelaxRatio);
|
||||
sbufWriteU8(dst, MIN(currentPidProfile->dtermSetpointWeight, 255));
|
||||
sbufWriteU8(dst, currentPidProfile->feedForwardTransition);
|
||||
sbufWriteU8(dst, 0); // was low byte of currentPidProfile->dtermSetpointWeight
|
||||
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
|
||||
sbufWriteU16(dst, currentPidProfile->itermThrottleThreshold);
|
||||
sbufWriteU16(dst, currentPidProfile->itermAcceleratorGain);
|
||||
sbufWriteU16(dst, currentPidProfile->dtermSetpointWeight);
|
||||
sbufWriteU16(dst, 0); // was currentPidProfile->dtermSetpointWeight
|
||||
sbufWriteU8(dst, currentPidProfile->iterm_rotation);
|
||||
#if defined(USE_SMART_FEEDFORWARD)
|
||||
sbufWriteU8(dst, currentPidProfile->smart_feedforward);
|
||||
|
|
@ -1333,6 +1333,9 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
|
|||
#else
|
||||
sbufWriteU8(dst, 0);
|
||||
#endif
|
||||
sbufWriteU16(dst, currentPidProfile->pid[PID_ROLL].F);
|
||||
sbufWriteU16(dst, currentPidProfile->pid[PID_PITCH].F);
|
||||
sbufWriteU16(dst, currentPidProfile->pid[PID_YAW].F);
|
||||
|
||||
break;
|
||||
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
|
||||
sbufReadU8(src); // reserved
|
||||
currentPidProfile->vbatPidCompensation = sbufReadU8(src);
|
||||
currentPidProfile->setpointRelaxRatio = sbufReadU8(src);
|
||||
currentPidProfile->dtermSetpointWeight = sbufReadU8(src);
|
||||
currentPidProfile->feedForwardTransition = sbufReadU8(src);
|
||||
sbufReadU8(src); // was low byte of currentPidProfile->dtermSetpointWeight
|
||||
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);
|
||||
}
|
||||
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
|
||||
currentPidProfile->iterm_rotation = sbufReadU8(src);
|
||||
#if defined(USE_SMART_FEEDFORWARD)
|
||||
|
|
@ -1883,6 +1886,10 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
#else
|
||||
sbufReadU8(src);
|
||||
#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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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_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) },
|
||||
{ "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, setpointRelaxRatio) },
|
||||
{ "dterm_setpoint_weight", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dtermSetpointWeight) },
|
||||
{ "feedforward_transition", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedForwardTransition) },
|
||||
{ "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) },
|
||||
{ "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) },
|
||||
{ "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) },
|
||||
{ "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) },
|
||||
{ "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) },
|
||||
{ "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) },
|
||||
{ "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) },
|
||||
{ "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) },
|
||||
{ "i_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].I) },
|
||||
|
|
|
|||
|
|
@ -282,7 +282,7 @@ static void osdFormatAltitudeString(char * buff, int altitude)
|
|||
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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -56,7 +56,7 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
|
|||
.rssi_offset = 0,
|
||||
.rssi_invert = 0,
|
||||
.rcInterpolation = RC_SMOOTHING_AUTO,
|
||||
.rcInterpolationChannels = INTERPOLATION_CHANNELS_RPT,
|
||||
.rcInterpolationChannels = INTERPOLATION_CHANNELS_RPYT,
|
||||
.rcInterpolationInterval = 19,
|
||||
.fpvCamAngleDegrees = 0,
|
||||
.airModeActivateThreshold = 32,
|
||||
|
|
|
|||
|
|
@ -139,8 +139,9 @@ void targetConfiguration(void)
|
|||
/* Setpoints */
|
||||
pidProfile->dterm_filter_type = FILTER_BIQUAD;
|
||||
pidProfile->dterm_notch_hz = 0;
|
||||
pidProfile->dtermSetpointWeight = 100;
|
||||
pidProfile->setpointRelaxRatio = 0;
|
||||
pidProfile->pid[PID_PITCH].F = 100;
|
||||
pidProfile->pid[PID_ROLL].F = 100;
|
||||
pidProfile->feedForwardTransition = 0;
|
||||
|
||||
/* Anti-Gravity */
|
||||
pidProfile->itermThrottleThreshold = 500;
|
||||
|
|
|
|||
|
|
@ -85,8 +85,9 @@ void targetConfiguration(void)
|
|||
pidProfile->pid[PID_LEVEL].P = 30;
|
||||
pidProfile->pid[PID_LEVEL].D = 30;
|
||||
|
||||
pidProfile->dtermSetpointWeight = 200;
|
||||
pidProfile->setpointRelaxRatio = 50;
|
||||
pidProfile->pid[PID_PITCH].F = 200;
|
||||
pidProfile->pid[PID_ROLL].F = 200;
|
||||
pidProfile->feedForwardTransition = 50;
|
||||
}
|
||||
|
||||
for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) {
|
||||
|
|
|
|||
|
|
@ -84,10 +84,10 @@ int loopIter = 0;
|
|||
void setDefaultTestSettings(void) {
|
||||
pgResetAll();
|
||||
pidProfile = pidProfilesMutable(1);
|
||||
pidProfile->pid[PID_ROLL] = { 40, 40, 30 };
|
||||
pidProfile->pid[PID_PITCH] = { 58, 50, 35 };
|
||||
pidProfile->pid[PID_YAW] = { 70, 45, 0 };
|
||||
pidProfile->pid[PID_LEVEL] = { 50, 50, 75 };
|
||||
pidProfile->pid[PID_ROLL] = { 40, 40, 30, 65 };
|
||||
pidProfile->pid[PID_PITCH] = { 58, 50, 35, 60 };
|
||||
pidProfile->pid[PID_YAW] = { 70, 45, 20, 60 };
|
||||
pidProfile->pid[PID_LEVEL] = { 50, 50, 75, 0 };
|
||||
|
||||
pidProfile->pidSumLimit = PIDSUM_LIMIT;
|
||||
pidProfile->pidSumLimitYaw = PIDSUM_LIMIT_YAW;
|
||||
|
|
@ -101,8 +101,7 @@ void setDefaultTestSettings(void) {
|
|||
pidProfile->vbatPidCompensation = 0;
|
||||
pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
|
||||
pidProfile->levelAngleLimit = 55;
|
||||
pidProfile->setpointRelaxRatio = 100;
|
||||
pidProfile->dtermSetpointWeight = 0;
|
||||
pidProfile->feedForwardTransition = 100;
|
||||
pidProfile->yawRateAccelLimit = 100;
|
||||
pidProfile->rateAccelLimit = 0;
|
||||
pidProfile->antiGravityMode = ANTI_GRAVITY_SMOOTH;
|
||||
|
|
@ -267,7 +266,7 @@ TEST(pidControllerTest, testPidLoop) {
|
|||
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_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
|
||||
simulatedSetpointRate[FD_ROLL] = 100;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue