Skip to content
New issue

Have a question about this project? # for a free GitHub account to open an issue and contact its maintainers and the community.

#

By clicking “#”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? # to your account

Fix simple Dterm diferentiator #4538

Merged
merged 4 commits into from
Mar 27, 2019
Merged

Fix simple Dterm diferentiator #4538

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
29482eb
Fix FIR init
DzikuVx Mar 24, 2019
d927b2a
Simple diferentiator
DzikuVx Mar 24, 2019
500c4cd
Fix diferentiator to use 3 axises
DzikuVx Mar 24, 2019
c19fcbf
use FIR to compute simple dterm
DzikuVx Mar 24, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
53 changes: 33 additions & 20 deletions src/main/flight/pid.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -94,6 +94,7 @@ STATIC_FASTRAM filterApplyFnPtr notchFilterApplyFn;

extern float dT;

STATIC_FASTRAM bool pidFiltersConfigured = false;
FASTRAM float headingHoldCosZLimit;
FASTRAM int16_t headingHoldTarget;
STATIC_FASTRAM pt1Filter_t headingHoldRateFilter;
Expand Down Expand Up @@ -209,16 +210,6 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,

void pidInit(void)
{
// 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
static const float dtermCoeffs[PID_GYRO_RATE_BUF_LENGTH] = {5.0f/8, 2.0f/8, -8.0f/8, -2.0f/8, 3.0f/8};
if (pidProfile()->use_dterm_fir_filter) {
for (int axis = 0; axis < 3; ++ axis) {
firFilterInit(&pidState[axis].gyroRateFilter, pidState[axis].gyroRateBuf, PID_GYRO_RATE_BUF_LENGTH, dtermCoeffs);
}
}

pidResetTPAFilter();

// Calculate max overall tilt (max pitch + max roll combined) as a limit to heading hold
Expand All @@ -236,6 +227,30 @@ 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);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

In case of !use_dterm_fir_filter we can use length of 2 instead of PID_GYRO_RATE_BUF_LENGTH

}

#ifdef USE_DTERM_NOTCH
notchFilterApplyFn = nullFilterApply;
if (pidProfile()->dterm_soft_notch_hz != 0) {
Expand All @@ -251,6 +266,8 @@ bool pidInitFilters(void)
biquadFilterInitLPF(&pidState[axis].deltaLpfState, pidProfile()->dterm_lpf_hz, refreshRate);
}

pidFiltersConfigured = true;

return true;
}

Expand Down Expand Up @@ -570,16 +587,8 @@ static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, fl
deltaFiltered = biquadFilterApply(&pidState->deltaLpfState, deltaFiltered);
}

/*
* Apply "Classical" INAV noise robust differentiator if configured to do so
* This filter is not configurable in any way, it only can be enabled or disabled
*/
if (pidProfile()->use_dterm_fir_filter) {
firFilterUpdate(&pidState->gyroRateFilter, deltaFiltered);
newDTerm = firFilterApply(&pidState->gyroRateFilter);
} else {
newDTerm = deltaFiltered;
}
firFilterUpdate(&pidState->gyroRateFilter, deltaFiltered);
newDTerm = firFilterApply(&pidState->gyroRateFilter);

// Calculate derivative
newDTerm = newDTerm * (pidState->kD / dT);
Expand Down Expand Up @@ -798,6 +807,10 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng

void FAST_CODE pidController(void)
{
if (!pidFiltersConfigured) {
return;
}

bool canUseFpvCameraMix = true;
uint8_t headingHoldState = getHeadingHoldState();

Expand Down