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

Merge pull request #6355 from etracer65/pid_feedforward

Browse source 
PID controller feedforward
This commit is contained in:
Michael Keller 2018-07-22 11:34:28 +12:00 • committed by GitHub
commit 5e2fefe036
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
14 changed files with 266 additions and 160 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", 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);

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

@ -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 },

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

@ -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

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

@ -203,11 +203,11 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
.mode = ADJUSTMENT_MODE_STEP,
.data = { .step = 1 }
}, {
.adjustmentFunction = ADJUSTMENT_D_SETPOINT,
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_F,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .step = 1 }
}, {
.adjustmentFunction = ADJUSTMENT_D_SETPOINT_TRANSITION,
.adjustmentFunction = ADJUSTMENT_FEEDFORWARD_TRANSITION,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .step = 1 }
}, {
@ -218,6 +218,18 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
.adjustmentFunction = ADJUSTMENT_PID_AUDIO,
.mode = ADJUSTMENT_MODE_SELECT,
.data = { .switchPositions = ARRAYLEN(pidAudioPositionToModeMap) }
}, {
.adjustmentFunction = ADJUSTMENT_PITCH_F,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .step = 1 }
}, {
.adjustmentFunction = ADJUSTMENT_ROLL_F,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .step = 1 }
}, {
.adjustmentFunction = ADJUSTMENT_YAW_F,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .step = 1 }
}
};
@ -244,14 +256,17 @@ static const char * const adjustmentLabels[] = {
"ROLL I",
"ROLL D",
"RC RATE YAW",
"D SETPOINT",
"D SETPOINT TRANSITION",
"PITCH/ROLL F",
"FF TRANSITION",
"HORIZON STRENGTH",
"ROLL RC RATE",
"PITCH RC RATE",
"ROLL RC EXPO",
"PITCH RC EXPO",
"PID AUDIO",
"PITCH F",
"ROLL F",
"YAW F"
};
static int adjustmentRangeNameIndex = 0;
@ -405,15 +420,31 @@ static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t a
controlRateConfig->rcRates[FD_YAW] = newValue;
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;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue);
case ADJUSTMENT_PITCH_ROLL_F:
case ADJUSTMENT_PITCH_F:
newValue = constrain(pidProfile->pid[PID_PITCH].F + delta, 0, 2000);
pidProfile->pid[PID_PITCH].F = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_F, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_F) {
break;
}
// fall through for combined ADJUSTMENT_PITCH_ROLL_F
FALLTHROUGH;
case ADJUSTMENT_ROLL_F:
newValue = constrain(pidProfile->pid[PID_ROLL].F + delta, 0, 2000);
pidProfile->pid[PID_ROLL].F = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_F, 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;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue);
case ADJUSTMENT_YAW_F:
newValue = constrain(pidProfile->pid[PID_YAW].F + delta, 0, 2000);
pidProfile->pid[PID_YAW].F = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_F, newValue);
break;
case ADJUSTMENT_FEEDFORWARD_TRANSITION:
newValue = constrain(pidProfile->feedForwardTransition + delta, 1, 100); // FIXME magic numbers repeated in cli.c
pidProfile->feedForwardTransition = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FEEDFORWARD_TRANSITION, newValue);
break;
default:
newValue = -1;
@ -554,15 +585,31 @@ static int applyAbsoluteAdjustment(controlRateConfig_t *controlRateConfig, adjus
controlRateConfig->rcRates[FD_YAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue);
break;
case ADJUSTMENT_D_SETPOINT:
newValue = constrain(value, 0, 2000); // FIXME magic numbers repeated in cli.c
pidProfile->dtermSetpointWeight = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue);
case ADJUSTMENT_PITCH_ROLL_F:
case ADJUSTMENT_PITCH_F:
newValue = constrain(value, 0, 2000);
pidProfile->pid[PID_PITCH].F = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_F, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_F) {
break;
}
// fall through for combined ADJUSTMENT_PITCH_ROLL_F
FALLTHROUGH;
case ADJUSTMENT_ROLL_F:
newValue = constrain(value, 0, 2000);
pidProfile->pid[PID_ROLL].F = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_F, newValue);
break;
case ADJUSTMENT_D_SETPOINT_TRANSITION:
case ADJUSTMENT_YAW_F:
newValue = constrain(value, 0, 2000);
pidProfile->pid[PID_YAW].F = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_F, newValue);
break;
case ADJUSTMENT_FEEDFORWARD_TRANSITION:
newValue = constrain(value, 1, 100); // FIXME magic numbers repeated in cli.c
pidProfile->setpointRelaxRatio = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue);
pidProfile->feedForwardTransition = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FEEDFORWARD_TRANSITION, newValue);
break;
default:
newValue = -1;

7
src/main/fc/rc_adjustments.h
View file

@ -47,14 +47,17 @@ typedef enum {
ADJUSTMENT_ROLL_I,
ADJUSTMENT_ROLL_D,
ADJUSTMENT_RC_RATE_YAW,
ADJUSTMENT_D_SETPOINT,
ADJUSTMENT_D_SETPOINT_TRANSITION,
ADJUSTMENT_PITCH_ROLL_F,
ADJUSTMENT_FEEDFORWARD_TRANSITION,
ADJUSTMENT_HORIZON_STRENGTH,
ADJUSTMENT_ROLL_RC_RATE,
ADJUSTMENT_PITCH_RC_RATE,
ADJUSTMENT_ROLL_RC_EXPO,
ADJUSTMENT_PITCH_RC_EXPO,
ADJUSTMENT_PID_AUDIO,
ADJUSTMENT_PITCH_F,
ADJUSTMENT_ROLL_F,
ADJUSTMENT_YAW_F,
ADJUSTMENT_FUNCTION_COUNT
} adjustmentFunction_e;

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
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;
}

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

@ -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);

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
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);

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_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) },

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

@ -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);
}

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

@ -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,

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

@ -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;

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].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++) {

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

@ -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;