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

Improve soft filtering function for reuse

Browse source 
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:
borisbstyle 2015-06-30 11:05:13 +02:00
parent 637fd64f36
commit fdcfe71b73
9 changed files with 86 additions and 68 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -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;