diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 4e38d285f3..bf6a12fce0 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -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); diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 4846903f14..b38553b18e 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -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 }, diff --git a/src/main/fc/config.c b/src/main/fc/config.c index cf732f77ee..28475b3f45 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -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 diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index fcfcb4211f..5cc2cc471f 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -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: diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9a4eb3e82e..43a4345b40 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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; } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 25a17ec201..8ac00a0511 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -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); \ No newline at end of file +bool pidAntiGravityEnabled(void); diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index 489154e68a..e092dcf5b2 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -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); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 0b5fcb1c16..9e27e31fbc 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -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) }, diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 9bb00d2829..4889d6a237 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -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); } diff --git a/src/main/pg/rx.c b/src/main/pg/rx.c index 2bef6d3d15..cb944799e2 100644 --- a/src/main/pg/rx.c +++ b/src/main/pg/rx.c @@ -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, diff --git a/src/main/target/ALIENWHOOP/config.c b/src/main/target/ALIENWHOOP/config.c index 2983443e8f..968d9125b9 100644 --- a/src/main/target/ALIENWHOOP/config.c +++ b/src/main/target/ALIENWHOOP/config.c @@ -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; diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index 9ee4aa1793..89266f32aa 100644 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -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++) { diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index 5cf65339f9..f222b54115 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -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;