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

rpm telemetry based notch filter

Browse source
This commit is contained in:
Thorsten Laux 2019-01-03 14:56:17 +01:00
parent 00e0248988
commit 8d4ed72e13
14 changed files with 300 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -56,6 +56,9 @@
#include "sensors/gyro.h"
#include "sensors/acceleration.h"
#ifdef USE_RPM_FILTER
#include "sensors/rpm_filter.h"
#endif
const char pidNames[] =
"ROLL;"
@ -604,6 +607,9 @@ void pidInit(const pidProfile_t *pidProfile)
pidSetTargetLooptime(gyro.targetLooptime * pidConfig()->pid_process_denom); // Initialize pid looptime
pidInitFilters(pidProfile);
pidInitConfig(pidProfile);
#ifdef USE_RPM_FILTER
rpmFilterInit(rpmFilterConfig());
#endif
}
#ifdef USE_ACRO_TRAINER
@ -1103,12 +1109,20 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
// Precalculate gyro deta for D-term here, this allows loop unrolling
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] = gyro.gyroADCf[axis];
#ifdef USE_RPM_FILTER
gyroRateDterm[axis] = rpmFilterDterm(axis,gyroRateDterm[axis]);
#endif
gyroRateDterm[axis] = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyroRateDterm[axis]);
gyroRateDterm[axis] = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateDterm[axis]);
gyroRateDterm[axis] = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateDterm[axis]);
}
rotateItermAndAxisError();
#ifdef USE_RPM_FILTER
rpmFilterUpdate();
#endif
// ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {