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

Added filtering for parts of the notch filter that are below the nyquist frequency.

Browse source
This commit is contained in:
mikeller 2017-05-16 22:57:15 +12:00
parent 2463d5164e
commit 9de89cd464
2 changed files with 38 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -158,6 +158,8 @@ static void *ptermYawFilter;
void pidInitFilters(const pidProfile_t *pidProfile) 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
static biquadFilter_t biquadFilterNotch[2]; static biquadFilter_t biquadFilterNotch[2];
static pt1Filter_t pt1Filter[2]; static pt1Filter_t pt1Filter[2];
static biquadFilter_t biquadFilter[2]; static biquadFilter_t biquadFilter[2];
@ -166,16 +168,25 @@ void pidInitFilters(const pidProfile_t *pidProfile)
uint32_t pidFrequencyNyquist = (1.0f / dT) / 2; // No rounding needed uint32_t pidFrequencyNyquist = (1.0f / dT) / 2; // No rounding needed
BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2 float dTermNotchHz;
if (pidProfile->dterm_notch_hz <= pidFrequencyNyquist) {
dTermNotchHz = pidProfile->dterm_notch_hz;
} else {
if (pidProfile->dterm_notch_cutoff < pidFrequencyNyquist) {
dTermNotchHz = pidFrequencyNyquist;
} else {
dTermNotchHz = 0;
}
}
if (pidProfile->dterm_notch_hz == 0 || pidProfile->dterm_notch_hz > pidFrequencyNyquist) { if (!dTermNotchHz) {
dtermNotchFilterApplyFn = nullFilterApply; dtermNotchFilterApplyFn = nullFilterApply;
} else { } else {
dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff); 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_PITCH; axis++) {
dtermFilterNotch[axis] = &biquadFilterNotch[axis]; dtermFilterNotch[axis] = &biquadFilterNotch[axis];
biquadFilterInit(dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH); biquadFilterInit(dtermFilterNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH);
} }
} }

28
src/main/sensors/gyro.c
View file

@ -364,7 +364,7 @@ bool gyroInit(void)
void gyroInitFilterLpf(gyroSensor_t *gyroSensor, uint8_t lpfHz) void gyroInitFilterLpf(gyroSensor_t *gyroSensor, uint8_t lpfHz)
{ {
gyroSensor->softLpfFilterApplyFn = nullFilterApply; gyroSensor->softLpfFilterApplyFn = nullFilterApply;
const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed const uint32_t gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
if (lpfHz && lpfHz <= gyroFrequencyNyquist) { // Initialisation needs to happen once samplingrate is known if (lpfHz && lpfHz <= gyroFrequencyNyquist) { // Initialisation needs to happen once samplingrate is known
switch (gyroConfig()->gyro_soft_lpf_type) { switch (gyroConfig()->gyro_soft_lpf_type) {
@ -394,11 +394,27 @@ void gyroInitFilterLpf(gyroSensor_t *gyroSensor, uint8_t lpfHz)
} }
} }
static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notchCutoffHz)
{
const uint32_t gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
if (notchHz > gyroFrequencyNyquist) {
if (notchCutoffHz < gyroFrequencyNyquist) {
notchHz = gyroFrequencyNyquist;
} else {
notchHz = 0;
}
}
return notchHz;
}
void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz) void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz)
{ {
gyroSensor->notchFilter1ApplyFn = nullFilterApply; gyroSensor->notchFilter1ApplyFn = nullFilterApply;
const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed
if (notchHz && notchHz <= gyroFrequencyNyquist) { notchHz = calculateNyquistAdjustedNotchHz(notchHz, notchCutoffHz);
if (notchHz) {
gyroSensor->notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; gyroSensor->notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz); const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
@ -410,8 +426,10 @@ void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t n
void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz) void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz)
{ {
gyroSensor->notchFilter2ApplyFn = nullFilterApply; gyroSensor->notchFilter2ApplyFn = nullFilterApply;
const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed
if (notchHz && notchHz <= gyroFrequencyNyquist) { notchHz = calculateNyquistAdjustedNotchHz(notchHz, notchCutoffHz);
if (notchHz) {
gyroSensor->notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; gyroSensor->notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz); const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {