Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 09:16:01 +03:00
Code Issues Wiki Activity

Add setpoint derivative feed forward term to PID controller (#5642)

Browse source 
* Add setpoint derivative feed forward term to PID controller

* Drop Dterm setpoint weight, FIR filter and dterm notch

* Fix FALCORE build

* Rename Multirotor FeedForward to Control Derivative

* Apply Control Derivative results

* Free CCM on F3

* Rename CLI name

* Remove Omnibus F3 support

* Update CLI.md and rename setting
This commit is contained in:
Paweł Spychalski 2020-06-09 13:37:49 +02:00 • committed by GitHub
parent 9ba24af387
commit 839a877397
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
16 changed files with 85 additions and 149 deletions
Download patch file Download diff file Expand all files Collapse all files

2
docs/Board - OMNIBUS.md
View file

@ -1,5 +1,7 @@
# Board - OMNIBUS F3
> This board is not supported in recent INAV releases
## Hardware Features
Refer to the product web page:

5
docs/Cli.md
View file

@ -399,12 +399,15 @@ A shorter form is also supported to enable and disable functions using `serial <
| mc_p_pitch | 40 | Multicopter rate stabilisation P-gain for PITCH |
| mc_i_pitch | 30 | Multicopter rate stabilisation I-gain for PITCH |
| mc_d_pitch | 23 | Multicopter rate stabilisation D-gain for PITCH |
| mc_cd_pitch | 60 | Multicopter Control Derivative gain for PITCH |
| mc_p_roll | 40 | Multicopter rate stabilisation P-gain for ROLL |
| mc_i_roll | 30 | Multicopter rate stabilisation I-gain for ROLL |
| mc_d_roll | 23 | Multicopter rate stabilisation D-gain for ROLL |
| mc_cd_roll | 60 | Multicopter Control Derivative gain for ROLL |
| mc_p_yaw | 85 | Multicopter rate stabilisation P-gain for YAW |
| mc_i_yaw | 45 | Multicopter rate stabilisation I-gain for YAW |
| mc_d_yaw | 0 | Multicopter rate stabilisation D-gain for YAW |
| mc_cd_yaw | 60 | Multicopter Control Derivative gain for YAW |
| mc_p_level | 20 | Multicopter attitude stabilisation P-gain |
| mc_i_level | 15 | Multicopter attitude stabilisation low-pass filter cutoff |
| mc_d_level | 75 | Multicopter attitude stabilisation HORIZON transition point |
@ -505,7 +508,6 @@ A shorter form is also supported to enable and disable functions using `serial <
| baro_cal_tolerance | 150 | Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]. |
| mc_airmode_type | STICK_CENTER | Defines the Airmode state handling type for Multirotors. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `mc_airmode_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER** mode. |
| mc_airmode_threshold | 1300 | Defines airmode THROTTLE activation threshold when `mc_airmode_type` **THROTTLE_THRESHOLD** is used |
| use_dterm_fir_filter | ON | Setting to **OFF** disabled extra filter on Dterm. **OFF** offers faster Dterm and better inflight performance with a cost of being more sensitive to gyro noise. Small and relatively clean multirotors (7 inches and below) are suggested to use **OFF** setting. If motors are getting too hot, switch back to **ON** |
| sim_ground_station_number | Empty string | Number of phone that is used to communicate with SIM module. Messages / calls from other numbers are ignored. If undefined, can be set by calling or sending a message to the module. |
| sim_pin | Empty string | PIN code for the SIM module |
| sim_transmit_interval | 60 | Text message transmission interval in seconds for SIM module. Minimum value: 10 |
@ -521,3 +523,4 @@ A shorter form is also supported to enable and disable functions using `serial <
| antigravity_accelerator | 1 | |
| antigravity_cutoff_lpf_hz | 15 | Antigravity cutoff frequenct for Throtte filter. Antigravity is based on the difference between actual and filtered throttle input. The bigger is the difference, the bigger Antigravity gain |
| sim_pin | | PIN for GSM card module |
| mc_cd_lpf_hz | 30 | Cutoff frequency for Control Derivative. Lower value smoother reaction on fast stick movements. With higher values, response will be more aggressive, jerky |

2
make/release.mk
View file

@ -10,7 +10,7 @@ RELEASE_TARGETS += KAKUTEF4 KAKUTEF4V2 KAKUTEF7 KAKUTEF7MINI KAKUTEF7HDV
RELEASE_TARGETS += SPRACINGF3 SPRACINGF3EVO SPRACINGF3EVO_1SS SPRACINGF3MINI SPRACINGF4EVO SPRACINGF7DUAL
RELEASE_TARGETS += OMNIBUS AIRBOTF4 ASGARD32F4 ASGARD32F7 FIREWORKSV2 AIRBOTF7 OMNIBUSF7NANOV7
RELEASE_TARGETS += AIRBOTF4 ASGARD32F4 ASGARD32F7 FIREWORKSV2 AIRBOTF7 OMNIBUSF7NANOV7
RELEASE_TARGETS += OMNIBUSF4 OMNIBUSF4PRO OMNIBUSF4PRO_LEDSTRIPM5 DYSF4PRO DYSF4PROV2
RELEASE_TARGETS += OMNIBUSF4V3 OMNIBUSF4V3_S6_SS OMNIBUSF4V3_S5S6_SS OMNIBUSF4V3_S5_S6_2SS
RELEASE_TARGETS += OMNIBUSF7 OMNIBUSF7V2 OMNIBUSF7NXT YUPIF7

2
make/targets.mk
View file

@ -66,7 +66,7 @@ endif
GROUP_1_TARGETS := AIRHEROF3 AIRHEROF3_QUAD LUX_RACE SPARKY REVO SPARKY2 COLIBRI FALCORE FF_F35_LIGHTNING FF_FORTINIF4 FF_PIKOF4 FF_PIKOF4OSD
GROUP_2_TARGETS := SPRACINGF3 SPRACINGF3EVO SPRACINGF3EVO_1SS SPRACINGF3MINI SPRACINGF4EVO CLRACINGF4AIR CLRACINGF4AIRV2 BEEROTORF4 BETAFLIGHTF3 BETAFLIGHTF4 PIKOBLX
GROUP_3_TARGETS := OMNIBUS AIRBOTF4 BLUEJAYF4 OMNIBUSF4 OMNIBUSF4PRO FIREWORKSV2 SPARKY2 MATEKF405 OMNIBUSF7 DYSF4PRO OMNIBUSF4PRO_LEDSTRIPM5 OMNIBUSF7NXT OMNIBUSF7V2 ASGARD32F4
GROUP_3_TARGETS := AIRBOTF4 BLUEJAYF4 OMNIBUSF4 OMNIBUSF4PRO FIREWORKSV2 SPARKY2 MATEKF405 OMNIBUSF7 DYSF4PRO OMNIBUSF4PRO_LEDSTRIPM5 OMNIBUSF7NXT OMNIBUSF7V2 ASGARD32F4
GROUP_4_TARGETS := ANYFC ANYFCF7 ANYFCF7_EXTERNAL_BARO ALIENFLIGHTNGF7 PIXRACER YUPIF7 MATEKF405SE MATEKF411 MATEKF722 MATEKF405OSD MATEKF405_SERVOS6 NOX
GROUP_5_TARGETS := ASGARD32F7 CLRACINGF4AIRV3 DALRCF405 DALRCF722DUAL DYSF4PROV2 F4BY FISHDRONEF4 FOXEERF405 FOXEERF722DUAL FRSKYF3 FRSKYF4 FURYF3 FURYF3_SPIFLASH FURYF4OSD
GROUP_6_TARGETS := MAMBAF405 OMNIBUSF4V3 OMNIBUSF4V3_S6_SS OMNIBUSF4V3_S5S6_SS OMNIBUSF4V3_S5_S6_2SS AIKONF4

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

@ -1717,8 +1717,6 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_type", "%d", pidProfile()->dterm_lpf_type);
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf2_hz", "%d", pidProfile()->dterm_lpf2_hz);
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf2_type", "%d", pidProfile()->dterm_lpf2_type);
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz", "%d", pidProfile()->dterm_soft_notch_hz);
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff", "%d", pidProfile()->dterm_soft_notch_cutoff);
BLACKBOX_PRINT_HEADER_LINE("deadband", "%d", rcControlsConfig()->deadband);
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf", "%d", gyroConfig()->gyro_lpf);
@ -1747,7 +1745,6 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("acc_notch_hz", "%d", accelerometerConfig()->acc_notch_hz);
BLACKBOX_PRINT_HEADER_LINE("acc_notch_cutoff", "%d", accelerometerConfig()->acc_notch_cutoff);
BLACKBOX_PRINT_HEADER_LINE("gyro_stage2_lowpass_hz", "%d", gyroConfig()->gyro_stage2_lowpass_hz);
BLACKBOX_PRINT_HEADER_LINE("dterm_setpoint_weight", "%f", (double)pidProfile()->dterm_setpoint_weight);
BLACKBOX_PRINT_HEADER_LINE("pidSumLimit", "%d", pidProfile()->pidSumLimit);
BLACKBOX_PRINT_HEADER_LINE("pidSumLimitYaw", "%d", pidProfile()->pidSumLimitYaw);
BLACKBOX_PRINT_HEADER_LINE("axisAccelerationLimitYaw", "%d", pidProfile()->axisAccelerationLimitYaw);

1
src/main/build/debug.h
View file

@ -71,5 +71,6 @@ typedef enum {
DEBUG_DYNAMIC_FILTER,
DEBUG_DYNAMIC_FILTER_FREQUENCY,
DEBUG_IRLOCK,
DEBUG_CD,
DEBUG_COUNT
} debugType_e;

36
src/main/common/filter.c
View file

@ -232,39 +232,3 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
filter->y1 = y1;
filter->y2 = y2;
}
/*
* FIR filter
*/
void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength)
{
filter->buf = buf;
filter->bufLength = bufLength;
filter->coeffs = coeffs;
filter->coeffsLength = coeffsLength;
memset(filter->buf, 0, sizeof(float) * filter->bufLength);
}
/*
* FIR filter initialisation
* If FIR filter is just used for averaging, coeffs can be set to NULL
*/
void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs)
{
firFilterInit2(filter, buf, bufLength, coeffs, bufLength);
}
void firFilterUpdate(firFilter_t *filter, float input)
{
memmove(&filter->buf[1], &filter->buf[0], (filter->bufLength-1) * sizeof(float));
filter->buf[0] = input;
}
float firFilterApply(const firFilter_t *filter)
{
float ret = 0.0f;
for (int ii = 0; ii < filter->coeffsLength; ++ii) {
ret += filter->coeffs[ii] * filter->buf[ii];
}
return ret;
}

5
src/main/common/filter.h
View file

@ -81,8 +81,3 @@ float biquadFilterReset(biquadFilter_t *filter, float value);
float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
float filterGetNotchQ(float centerFrequencyHz, float cutoffFrequencyHz);
void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);
void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength);
void firFilterUpdate(firFilter_t *filter, float input);
float firFilterApply(const firFilter_t *filter);

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

@ -170,9 +170,6 @@ void validateAndFixConfig(void)
if (gyroConfig()->gyro_notch_cutoff >= gyroConfig()->gyro_notch_hz) {
gyroConfigMutable()->gyro_notch_hz = 0;
}
if (pidProfile()->dterm_soft_notch_cutoff >= pidProfile()->dterm_soft_notch_hz) {
pidProfileMutable()->dterm_soft_notch_hz = 0;
}
if (accelerometerConfig()->acc_notch_cutoff >= accelerometerConfig()->acc_notch_hz) {
accelerometerConfigMutable()->acc_notch_hz = 0;
}

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

@ -1157,8 +1157,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, pidProfile()->yaw_lpf_hz);
sbufWriteU16(dst, gyroConfig()->gyro_notch_hz);
sbufWriteU16(dst, gyroConfig()->gyro_notch_cutoff);
sbufWriteU16(dst, pidProfile()->dterm_soft_notch_hz); //BF: pidProfile()->dterm_notch_hz
sbufWriteU16(dst, pidProfile()->dterm_soft_notch_cutoff); //pidProfile()->dterm_notch_cutoff
sbufWriteU16(dst, 0); //BF: pidProfile()->dterm_notch_hz
sbufWriteU16(dst, 1); //pidProfile()->dterm_notch_cutoff
sbufWriteU16(dst, 0); //BF: masterConfig.gyro_soft_notch_hz_2
sbufWriteU16(dst, 1); //BF: masterConfig.gyro_soft_notch_cutoff_2
@ -1176,7 +1176,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU8(dst, 0); //BF: pidProfile()->deltaMethod
sbufWriteU8(dst, 0); //BF: pidProfile()->vbatPidCompensation
sbufWriteU8(dst, 0); //BF: pidProfile()->setpointRelaxRatio
sbufWriteU8(dst, constrain(pidProfile()->dterm_setpoint_weight * 100, 0, 255));
sbufWriteU8(dst, 0);
sbufWriteU16(dst, pidProfile()->pidSumLimit);
sbufWriteU8(dst, 0); //BF: pidProfile()->itermThrottleGain
@ -2044,8 +2044,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
return MSP_RESULT_ERROR;
}
if (dataSize >= 13) {
pidProfileMutable()->dterm_soft_notch_hz = constrain(sbufReadU16(src), 0, 500);
pidProfileMutable()->dterm_soft_notch_cutoff = constrain(sbufReadU16(src), 1, 500);
sbufReadU16(src);
sbufReadU16(src);
pidInitFilters();
} else {
return MSP_RESULT_ERROR;
@ -2082,7 +2082,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
sbufReadU8(src); //BF: pidProfileMutable()->deltaMethod
sbufReadU8(src); //BF: pidProfileMutable()->vbatPidCompensation
sbufReadU8(src); //BF: pidProfileMutable()->setpointRelaxRatio
pidProfileMutable()->dterm_setpoint_weight = constrainf(sbufReadU8(src) / 100.0f, 0.0f, 2.0f);
sbufReadU8(src);
pidProfileMutable()->pidSumLimit = sbufReadU16(src);
sbufReadU8(src); //BF: pidProfileMutable()->itermThrottleGain

32
src/main/fc/settings.yaml
View file

@ -84,7 +84,7 @@ tables:
"FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX",
"ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
"IRLOCK"]
"IRLOCK", "CD"]
- name: async_mode
values: ["NONE", "GYRO", "ALL"]
- name: aux_operator
@ -969,6 +969,10 @@ groups:
field: bank_mc.pid[PID_PITCH].D
min: 0
max: 200
- name: mc_cd_pitch
field: bank_mc.pid[PID_PITCH].FF
min: 0
max: 200
- name: mc_p_roll
field: bank_mc.pid[PID_ROLL].P
min: 0
@ -981,6 +985,10 @@ groups:
field: bank_mc.pid[PID_ROLL].D
min: 0
max: 200
- name: mc_cd_roll
field: bank_mc.pid[PID_ROLL].FF
min: 0
max: 200
- name: mc_p_yaw
field: bank_mc.pid[PID_YAW].P
min: 0
@ -993,6 +1001,10 @@ groups:
field: bank_mc.pid[PID_YAW].D
min: 0
max: 200
- name: mc_cd_yaw
field: bank_mc.pid[PID_YAW].FF
min: 0
max: 200
- name: mc_p_level
field: bank_mc.pid[PID_LEVEL].P
min: 0
@ -1073,15 +1085,9 @@ groups:
- name: dterm_lpf2_type
field: dterm_lpf2_type
table: filter_type
- name: use_dterm_fir_filter
field: use_dterm_fir_filter
type: bool
- name: yaw_lpf_hz
min: 0
max: 200
- name: dterm_setpoint_weight
min: 0
max: 2
- name: fw_iterm_throw_limit
field: fixedWingItermThrowLimit
min: FW_ITERM_THROW_LIMIT_MIN
@ -1102,14 +1108,6 @@ groups:
field: fixedWingItermLimitOnStickPosition
min: 0
max: 1
- name: dterm_notch_hz
field: dterm_soft_notch_hz
min: 0
max: 500
- name: dterm_notch_cutoff
field: dterm_soft_notch_cutoff
min: 1
max: 500
- name: pidsum_limit
field: pidSumLimit
min: PID_SUM_LIMIT_MIN
@ -1283,6 +1281,10 @@ groups:
- name: pid_type
field: pidControllerType
table: pidTypeTable
- name: mc_cd_lpf_hz
field: controlDerivativeLpfHz
min: 0
max: 200
- name: PG_PID_AUTOTUNE_CONFIG
type: pidAutotuneConfig_t

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

@ -63,6 +63,7 @@ typedef struct {
float kI; // Integral gain
float kD; // Derivative gain
float kFF; // Feed-forward gain
float kCD; // Control Derivative
float kT; // Back-calculation tracking gain
float gyroRate;
@ -85,22 +86,23 @@ typedef struct {
pt1Filter_t ptermLpfState;
filter_t dtermLpfState;
filter_t dtermLpf2State;
// Dterm notch filtering
biquadFilter_t deltaNotchFilter;
float stickPosition;
#ifdef USE_D_BOOST
float previousRateTarget;
float previousRateGyro;
#ifdef USE_D_BOOST
pt1Filter_t dBoostLpf;
biquadFilter_t dBoostGyroLpf;
#endif
uint16_t pidSumLimit;
filterApply4FnPtr ptermFilterApplyFn;
bool itermLimitActive;
biquadFilter_t rateTargetFilter;
} pidState_t;
STATIC_FASTRAM filterApplyFnPtr notchFilterApplyFn;
STATIC_FASTRAM bool pidFiltersConfigured = false;
static EXTENDED_FASTRAM float headingHoldCosZLimit;
static EXTENDED_FASTRAM int16_t headingHoldTarget;
@ -115,8 +117,7 @@ FASTRAM int16_t axisPID[FLIGHT_DYNAMICS_INDEX_COUNT];
int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_I[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_D[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_Setpoint[FLIGHT_DYNAMICS_INDEX_COUNT];
#endif
STATIC_FASTRAM pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
static EXTENDED_FASTRAM pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
static EXTENDED_FASTRAM pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
static EXTENDED_FASTRAM uint8_t itermRelax;
static EXTENDED_FASTRAM uint8_t itermRelaxType;
@ -148,15 +149,16 @@ typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t
static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn;
static EXTENDED_FASTRAM bool levelingEnabled = false;
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 12);
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 13);
PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.bank_mc = {
.pid = {
[PID_ROLL] = { 40, 30, 23, 0 },
[PID_PITCH] = { 40, 30, 23, 0 },
[PID_YAW] = { 85, 45, 0, 0 },
[PID_ROLL] = { 40, 30, 23, 60 },
[PID_PITCH] = { 40, 30, 23, 60 },
[PID_YAW] = { 85, 45, 0, 60 },
[PID_LEVEL] = {
.P = 20, // Self-level strength
.I = 15, // Self-leveing low-pass frequency (0 - disabled)
@ -230,15 +232,11 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
}
},
.dterm_soft_notch_hz = 0,
.dterm_soft_notch_cutoff = 1,
.dterm_lpf_type = 1, //Default to BIQUAD
.dterm_lpf_hz = 40,
.dterm_lpf2_type = 1, //Default to BIQUAD
.dterm_lpf2_hz = 0, // Off by default
.yaw_lpf_hz = 0,
.dterm_setpoint_weight = 1.0f,
.use_dterm_fir_filter = 1,
.itermWindupPointPercent = 50, // Percent
@ -270,6 +268,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.antigravityCutoff = ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF,
.pidControllerType = PID_TYPE_AUTO,
.navFwPosHdgPidsumLimit = PID_SUM_LIMIT_YAW_DEFAULT,
.controlDerivativeLpfHz = 30,
);
bool pidInitFilters(void)
@ -280,38 +279,6 @@ bool pidInitFilters(void)
return false;
}
static float dtermCoeffs[PID_GYRO_RATE_BUF_LENGTH];
if (pidProfile()->use_dterm_fir_filter) {
// Calculate derivative using 5-point noise-robust differentiators without time delay (one-sided or forward filters)
// by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
// h[0] = 5/8, h[-1] = 1/4, h[-2] = -1, h[-3] = -1/4, h[-4] = 3/8
dtermCoeffs[0] = 5.0f/8;
dtermCoeffs[1] = 2.0f/8;
dtermCoeffs[2] = -8.0f/8;
dtermCoeffs[3] = -2.0f/8;
dtermCoeffs[4] = 3.0f/8;
} else {
//simple d(t) - d(t-1) differentiator
dtermCoeffs[0] = 1.0f;
dtermCoeffs[1] = -1.0f;
dtermCoeffs[2] = 0.0f;
dtermCoeffs[3] = 0.0f;
dtermCoeffs[4] = 0.0f;
}
for (int axis = 0; axis < 3; ++ axis) {
firFilterInit(&pidState[axis].gyroRateFilter, pidState[axis].gyroRateBuf, PID_GYRO_RATE_BUF_LENGTH, dtermCoeffs);
}
notchFilterApplyFn = nullFilterApply;
if (pidProfile()->dterm_soft_notch_hz != 0) {
notchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < 3; ++ axis) {
biquadFilterInitNotch(&pidState[axis].deltaNotchFilter, refreshRate, pidProfile()->dterm_soft_notch_hz, pidProfile()->dterm_soft_notch_cutoff);
}
}
// Init other filters
if (pidProfile()->dterm_lpf_hz) {
for (int axis = 0; axis < 3; ++ axis) {
@ -348,6 +315,12 @@ bool pidInitFilters(void)
}
#endif
if (pidProfile()->controlDerivativeLpfHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&pidState[axis].rateTargetFilter, pidProfile()->controlDerivativeLpfHz, getLooptime());
}
}
pidFiltersConfigured = true;
return true;
@ -496,6 +469,7 @@ void FAST_CODE NOINLINE updatePIDCoefficients(float dT)
pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER * tpaFactor;
pidState[axis].kD = 0.0f;
pidState[axis].kFF = pidBank()->pid[axis].FF / FP_PID_RATE_FF_MULTIPLIER * tpaFactor;
pidState[axis].kCD = 0.0f;
pidState[axis].kT = 0.0f;
}
else {
@ -503,6 +477,7 @@ void FAST_CODE NOINLINE updatePIDCoefficients(float dT)
pidState[axis].kP = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * axisTPA;
pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER;
pidState[axis].kD = pidBank()->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * axisTPA;
pidState[axis].kCD = pidBank()->pid[axis].FF / FP_PID_RATE_D_FF_MULTIPLIER * axisTPA;
pidState[axis].kFF = 0.0f;
// Tracking anti-windup requires P/I/D to be all defined which is only true for MC
@ -591,7 +566,7 @@ bool isFixedWingItermLimitActive(float stickPosition)
* Iterm anti windup whould be active only when pilot controls the rotation
* velocity directly, not when ANGLE or HORIZON are used
*/
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
if (levelingEnabled) {
return false;
}
@ -689,9 +664,6 @@ static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) {
dBoost = scaleRangef(acceleration, 0.0f, dBoostMaxAtAlleceleration, 1.0f, dBoostFactor);
dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT);
dBoost = constrainf(dBoost, 1.0f, dBoostFactor);
pidState->previousRateTarget = pidState->rateTarget;
pidState->previousRateGyro = pidState->gyroRate;
}
return dBoost;
@ -709,34 +681,38 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
const float rateError = pidState->rateTarget - pidState->gyroRate;
const float newPTerm = pTermProcess(pidState, rateError, dT);
const float rateTargetDelta = pidState->rateTarget - pidState->previousRateTarget;
const float rateTargetDeltaFiltered = biquadFilterApply(&pidState->rateTargetFilter, rateTargetDelta);
/*
* Compute Control Derivative
* CD is enabled only when ANGLE and HORIZON modes are not enabled!
*/
float newCDTerm;
if (levelingEnabled) {
newCDTerm = 0.0F;
} else {
newCDTerm = rateTargetDeltaFiltered * (pidState->kCD / dT);
}
DEBUG_SET(DEBUG_CD, axis, newCDTerm);
// Calculate new D-term
float newDTerm;
if (pidState->kD == 0) {
// optimisation for when D8 is zero, often used by YAW axis
newDTerm = 0;
} else {
// Calculate delta for Dterm calculation. Apply filters before derivative to minimize effects of dterm kick
const float deltaFiltered = pidProfile()->dterm_setpoint_weight * pidState->rateTarget - pidState->gyroRate;
float delta = pidState->previousRateGyro - pidState->gyroRate;
firFilterUpdate(&pidState->gyroRateFilter, deltaFiltered);
newDTerm = firFilterApply(&pidState->gyroRateFilter);
// Apply D-term notch
newDTerm = notchFilterApplyFn(&pidState->deltaNotchFilter, newDTerm);
// Apply additional lowpass
newDTerm = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, newDTerm);
newDTerm = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, newDTerm);
delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);
delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta);
// Calculate derivative
newDTerm = newDTerm * (pidState->kD / dT) * applyDBoost(pidState, dT);
// Additionally constrain D
newDTerm = constrainf(newDTerm, -300.0f, 300.0f);
newDTerm = delta * (pidState->kD / dT) * applyDBoost(pidState, dT);
}
// TODO: Get feedback from mixer on available correction range for each axis
const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf;
const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm;
const float newOutputLimited = constrainf(newOutput, -pidState->pidSumLimit, +pidState->pidSumLimit);
float itermErrorRate = rateError;
@ -760,6 +736,9 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
axisPID_D[axis] = newDTerm;
axisPID_Setpoint[axis] = pidState->rateTarget;
#endif
pidState->previousRateTarget = pidState->rateTarget;
pidState->previousRateGyro = pidState->gyroRate;
}
void updateHeadingHoldTarget(int16_t heading)
@ -995,6 +974,9 @@ void FAST_CODE pidController(float dT)
pidLevel(&pidState[FD_ROLL], FD_ROLL, horizonRateMagnitude, dT);
pidLevel(&pidState[FD_PITCH], FD_PITCH, horizonRateMagnitude, dT);
canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with ANGLE/HORIZON
levelingEnabled = true;
} else {
levelingEnabled = false;
}
if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) {

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

@ -45,6 +45,7 @@ FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13
#define FP_PID_RATE_P_MULTIPLIER 31.0f
#define FP_PID_RATE_I_MULTIPLIER 4.0f
#define FP_PID_RATE_D_MULTIPLIER 1905.0f
#define FP_PID_RATE_D_FF_MULTIPLIER 7270.0f
#define FP_PID_LEVEL_P_MULTIPLIER 6.56f // Level P gain units is [1/sec] and angle error is [deg] => [deg/s]
#define FP_PID_YAWHOLD_P_MULTIPLIER 80.0f
@ -104,17 +105,12 @@ typedef struct pidProfile_s {
pidBank_t bank_fw;
pidBank_t bank_mc;
uint16_t dterm_soft_notch_hz; // Dterm Notch frequency
uint16_t dterm_soft_notch_cutoff; // Dterm Notch Cutoff frequency
uint8_t dterm_lpf_type; // Dterm LPF type: PT1, BIQUAD
uint16_t dterm_lpf_hz;
uint8_t dterm_lpf2_type; // Dterm LPF type: PT1, BIQUAD
uint16_t dterm_lpf2_hz;
uint8_t use_dterm_fir_filter; // Use classical INAV FIR differentiator. Very noise robust, can be quite slowish
uint8_t yaw_lpf_hz;
uint8_t heading_hold_rate_limit; // Maximum rotation rate HEADING_HOLD mode can feed to yaw rate PID controller
@ -126,7 +122,6 @@ typedef struct pidProfile_s {
int16_t max_angle_inclination[ANGLE_INDEX_COUNT]; // Max possible inclination (roll and pitch axis separately
float dterm_setpoint_weight;
uint16_t pidSumLimit;
uint16_t pidSumLimitYaw;
@ -151,6 +146,7 @@ typedef struct pidProfile_s {
uint8_t antigravityCutoff;
uint16_t navFwPosHdgPidsumLimit;
uint8_t controlDerivativeLpfHz;
} pidProfile_t;
typedef struct pidAutotuneConfig_s {

4
src/main/io/osd_dji_hd.c
View file

@ -568,8 +568,8 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
sbufWriteU16(dst, pidProfile()->yaw_lpf_hz); // BF: currentPidProfile->yaw_lowpass_hz
sbufWriteU16(dst, gyroConfig()->gyro_notch_hz); // BF: gyroConfig()->gyro_soft_notch_hz_1
sbufWriteU16(dst, gyroConfig()->gyro_notch_cutoff); // BF: gyroConfig()->gyro_soft_notch_cutoff_1
sbufWriteU16(dst, pidProfile()->dterm_soft_notch_hz); // BF: currentPidProfile->dterm_notch_hz
sbufWriteU16(dst, pidProfile()->dterm_soft_notch_cutoff); // BF: currentPidProfile->dterm_notch_cutoff
sbufWriteU16(dst, 0); // BF: currentPidProfile->dterm_notch_hz
sbufWriteU16(dst, 1); // BF: currentPidProfile->dterm_notch_cutoff
sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_soft_notch_hz_2
sbufWriteU16(dst, 1); // BF: gyroConfig()->gyro_soft_notch_cutoff_2
sbufWriteU8(dst, 0); // BF: currentPidProfile->dterm_filter_type

3
src/main/target/FALCORE/config.c
View file

@ -167,9 +167,6 @@ void targetConfiguration(void)
pidProfileMutable()->max_angle_inclination[FD_PITCH] = 300;
pidProfileMutable()->dterm_lpf_hz = 70;
pidProfileMutable()->yaw_lpf_hz = 35;
pidProfileMutable()->dterm_setpoint_weight = 0;
pidProfileMutable()->dterm_soft_notch_hz = 0;
pidProfileMutable()->dterm_soft_notch_cutoff = 1;
pidProfileMutable()->pidSumLimit = 500;
pidProfileMutable()->axisAccelerationLimitRollPitch = 0;
pidProfileMutable()->axisAccelerationLimitYaw = 10000;

0
src/main/target/OMNIBUS/target.mk → src/main/target/OMNIBUS/target.mk_
View file