mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 00:05:33 +03:00
Improve soft filtering function for reuse
Reorder serial.c *_cut_hz parameters Remove unnecessary dT calculation in luxfloat Restructured filter filter.h fix Luxfloat remove internal dT Void function for gyro fillter
This commit is contained in:
parent
637fd64f36
commit
fdcfe71b73
9 changed files with 86 additions and 68 deletions
|
@ -25,6 +25,7 @@
|
|||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/filter.h"
|
||||
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/accgyro.h"
|
||||
|
@ -42,12 +43,12 @@
|
|||
#include "flight/imu.h"
|
||||
#include "flight/navigation.h"
|
||||
#include "flight/autotune.h"
|
||||
#include "flight/filter.h"
|
||||
|
||||
#include "config/runtime_config.h"
|
||||
|
||||
extern uint16_t cycleTime;
|
||||
extern uint8_t motorCount;
|
||||
extern float dT;
|
||||
|
||||
int16_t heading;
|
||||
int16_t axisPID[3];
|
||||
|
@ -112,12 +113,9 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
static float lastGyroRate[3];
|
||||
static float delta1[3], delta2[3];
|
||||
float delta, deltaSum;
|
||||
float dT;
|
||||
int axis;
|
||||
float horizonLevelStrength = 1;
|
||||
|
||||
dT = (float)cycleTime * 0.000001f;
|
||||
|
||||
if (FLIGHT_MODE(HORIZON_MODE)) {
|
||||
|
||||
// Figure out the raw stick positions
|
||||
|
@ -190,7 +188,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
|
||||
// Pterm low pass
|
||||
if (pidProfile->pterm_cut_hz) {
|
||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
|
||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
|
||||
}
|
||||
// -----calculate I component. Note that PIDweight is divided by 10, because it is simplified formule from the previous multiply by 10
|
||||
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * PIDweight[axis] / 10, -250.0f, 250.0f);
|
||||
|
@ -213,7 +211,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
|
||||
// Dterm low pass
|
||||
if (pidProfile->dterm_cut_hz) {
|
||||
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz);
|
||||
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
|
||||
}
|
||||
|
||||
DTerm = constrainf((deltaSum / 3.0f) * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
|
||||
|
@ -299,7 +297,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
|
||||
// Pterm low pass
|
||||
if (pidProfile->pterm_cut_hz) {
|
||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
|
||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
|
||||
}
|
||||
|
||||
delta = (gyroADC[axis] - lastGyro[axis]) / 4;
|
||||
|
@ -310,7 +308,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
|
||||
// Dterm low pass
|
||||
if (pidProfile->dterm_cut_hz) {
|
||||
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz);
|
||||
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
|
||||
}
|
||||
|
||||
DTerm = (deltaSum * dynD8[axis]) / 32;
|
||||
|
@ -389,7 +387,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
|
|||
|
||||
// Pterm low pass
|
||||
if (pidProfile->pterm_cut_hz) {
|
||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
|
||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
|
||||
}
|
||||
|
||||
delta = (gyroADC[axis] - lastGyro[axis]) / 4; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
|
||||
|
@ -400,7 +398,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
|
|||
|
||||
// Dterm low pass
|
||||
if (pidProfile->dterm_cut_hz) {
|
||||
DTerm = filterApplyPt1(DTerm, &DTermState[axis], pidProfile->dterm_cut_hz);
|
||||
DTerm = filterApplyPt1(DTerm, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
|
||||
}
|
||||
|
||||
DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5; // 32 bits is needed for calculation
|
||||
|
@ -513,7 +511,7 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
|
|||
|
||||
// Pterm low pass
|
||||
if (pidProfile->pterm_cut_hz) {
|
||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
|
||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
|
||||
}
|
||||
delta = (gyroADC[axis] - lastGyro[axis]) / 4;
|
||||
lastGyro[axis] = gyroADC[axis];
|
||||
|
@ -523,7 +521,7 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
|
|||
|
||||
// Dterm low pass
|
||||
if (pidProfile->dterm_cut_hz) {
|
||||
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz);
|
||||
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
|
||||
}
|
||||
|
||||
DTerm = (deltaSum * dynD8[axis]) / 32;
|
||||
|
@ -634,7 +632,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
|
|||
|
||||
// Pterm low pass
|
||||
if (pidProfile->pterm_cut_hz) {
|
||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
|
||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
|
||||
}
|
||||
|
||||
delta = (gyroADCQuant - lastGyro[axis]) / ACCDeltaTimeINS;
|
||||
|
@ -686,7 +684,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
|
|||
|
||||
// Pterm low pass
|
||||
if (pidProfile->pterm_cut_hz) {
|
||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
|
||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
|
||||
}
|
||||
|
||||
axisPID[FD_YAW] = PTerm + ITerm;
|
||||
|
@ -781,7 +779,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
|
|||
|
||||
// Pterm low pass
|
||||
if (pidProfile->pterm_cut_hz) {
|
||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
|
||||
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
|
||||
}
|
||||
|
||||
// -----calculate I component
|
||||
|
@ -810,7 +808,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
|
|||
|
||||
// Dterm delta low pass
|
||||
if (pidProfile->dterm_cut_hz) {
|
||||
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz);
|
||||
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
|
||||
}
|
||||
|
||||
DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue