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

Soft Filtering (Gyro, Dterm, Pterm)

Browse source 
pterm_cut_hz added

Let's play with this as well to get more noise filtered

Code Cleanup

Make filter more flexible for reuse

rewrite correction pterm

Define static delta in filter

Fix array count

ident

return function for filter

Filter Function enhanced

Full software filtering (DTerm, PTerm, Gyro, Acc)

Normalize Variables

Revert Back gyro settings

Bugfix gyro/acc filter // (MPU60xx equalize lpf settings)

Moved filtering to mw.c

This has been done to prevent reusing old cycletime for filter function.

acc_cut_hz removed (not needed)

Harakiri zero fix
This commit is contained in:
borisbstyle 2015-06-10 10:00:36 +02:00
parent 6323fd15d6
commit 10f2d35759
11 changed files with 156 additions and 33 deletions
Download patch file Download diff file Expand all files Collapse all files

1
Makefile
View file

@ -229,6 +229,7 @@ COMMON_SRC = build_config.c \
flight/imu.c \
flight/mixer.c \
flight/lowpass.c \
flight/filter.c \
drivers/bus_i2c_soft.c \
drivers/serial.c \
drivers/sound_beeper.c \

3
docs/Cli.md
View file

@ -246,6 +246,9 @@ Re-apply any new defaults as desired.
| `p_vel` | | 0 | 200 | 120 | Profile | UINT8 |
| `i_vel` | | 0 | 200 | 45 | Profile | UINT8 |
| `d_vel` | | 0 | 200 | 1 | Profile | UINT8 |
| `dterm_cut_hz` | Lowpass cutoff filter for Dterm for all PID controllers | 0 | 200 | 0 | Profile | UINT8 |
| `pterm_cut_hz` | Lowpass cutoff filter for Pterm for all PID controllers | 0 | 200 | 0 | Profile | UINT8 |
| `gyro_cut_hz` | Lowpass cutoff filter for gyro input | 0 | 200 | 0 | Profile | UINT8 | | 0 | 200 | 0 | Profile | UINT8 |
| `yaw_p_limit` | Limiter for yaw P term. This parameter is only affecting PID controller 3-5. To disable set to 500 (actual default). | 100 | 500 | 500 | Profile | UINT16 |
| `blackbox_rate_num` | | 1 | 32 | 1 | Master | UINT8 |
| `blackbox_rate_denom` | | 1 | 32 | 1 | Master | UINT8 |

3
src/main/config/config.c
View file

@ -171,6 +171,9 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->D8[PIDVEL] = 1;
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->dterm_cut_hz = 0;
pidProfile->pterm_cut_hz = 0;
pidProfile->gyro_cut_hz = 0;
pidProfile->P_f[ROLL] = 2.5f; // new PID with preliminary defaults test carefully
pidProfile->I_f[ROLL] = 0.6f;

8
src/main/drivers/accgyro_mpu6050.c
View file

@ -372,7 +372,9 @@ bool mpu6050GyroDetect(const mpu6050Config_t *configToUse, gyro_t *gyro, uint16_
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
if (lpf >= 188)
if (lpf == 256)
mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
else if (lpf >= 188)
mpuLowPassFilter = INV_FILTER_188HZ;
else if (lpf >= 98)
mpuLowPassFilter = INV_FILTER_98HZ;
@ -382,8 +384,10 @@ bool mpu6050GyroDetect(const mpu6050Config_t *configToUse, gyro_t *gyro, uint16_
mpuLowPassFilter = INV_FILTER_20HZ;
else if (lpf >= 10)
mpuLowPassFilter = INV_FILTER_10HZ;
else
else if (lpf > 0)
mpuLowPassFilter = INV_FILTER_5HZ;
else
mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
return true;
}

29
src/main/drivers/accgyro_spi_mpu6000.c
View file

@ -275,33 +275,22 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
int16_t data[3];
// default lpf is 42Hz
switch (lpf) {
case 256:
if (lpf == 256)
mpuLowPassFilter = BITS_DLPF_CFG_256HZ;
break;
case 188:
else if (lpf >= 188)
mpuLowPassFilter = BITS_DLPF_CFG_188HZ;
break;
case 98:
else if (lpf >= 98)
mpuLowPassFilter = BITS_DLPF_CFG_98HZ;
break;
default:
case 42:
else if (lpf >= 42)
mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
break;
case 20:
else if (lpf >= 20)
mpuLowPassFilter = BITS_DLPF_CFG_20HZ;
break;
case 10:
else if (lpf >= 10)
mpuLowPassFilter = BITS_DLPF_CFG_10HZ;
break;
case 5:
else if (lpf > 0)
mpuLowPassFilter = BITS_DLPF_CFG_5HZ;
break;
case 0:
mpuLowPassFilter = BITS_DLPF_CFG_2100HZ_NOLPF;
break;
}
else
mpuLowPassFilter = BITS_DLPF_CFG_256HZ;
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);

27
src/main/flight/filter.c Normal file
View file

@ -0,0 +1,27 @@
/*
* filter.c
*
* Created on: 24 jun. 2015
* Author: borisb
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include "common/axis.h"
#include "flight/filter.h"
extern uint16_t cycleTime;
// PT1 Low Pass filter
float filterApplyPt1(float input, filterStatePt1_t* state, uint8_t f_cut) {
float dT = (float)cycleTime * 0.000001f;
float RC= 1.0f / ( 2.0f * (float)M_PI * f_cut );
*state = *state + dT / (RC + dT) * (input - *state);
return *state;
}

11
src/main/flight/filter.h Normal file
View file

@ -0,0 +1,11 @@
/*
* filter.h
*
* Created on: 24 jun. 2015
* Author: borisb
*/
typedef float filterStatePt1_t;
float filterApplyPt1(float input, filterStatePt1_t* state, uint8_t f_cut);

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

@ -42,6 +42,7 @@
#include "flight/imu.h"
#include "flight/navigation.h"
#include "flight/autotune.h"
#include "flight/filter.h"
#include "config/runtime_config.h"
@ -93,6 +94,8 @@ void pidResetErrorGyro(void)
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static filterStatePt1_t PTermState[3], DTermState[3];
#ifdef AUTOTUNE
bool shouldAutotune(void)
{
@ -184,6 +187,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// -----calculate P component
PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
}
// -----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);
@ -202,6 +210,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
// Dterm low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz);
}
DTerm = constrainf((deltaSum / 3.0f) * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
// -----calculate total PID output
@ -282,11 +296,23 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
}
PTerm -= ((int32_t)gyroADC[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
}
delta = (gyroADC[axis] - lastGyro[axis]) / 4;
lastGyro[axis] = gyroADC[axis];
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
// Dterm low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz);
}
DTerm = (deltaSum * dynD8[axis]) / 32;
axisPID[axis] = PTerm + ITerm - DTerm;
@ -361,12 +387,22 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
PTerm -= ((int32_t)(gyroADC[axis] / 4) * dynP8[axis]) >> 6; // 32 bits is needed for calculation
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
}
delta = (gyroADC[axis] - lastGyro[axis]) / 4; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro[axis] = gyroADC[axis];
DTerm = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
// Dterm low pass
if (pidProfile->dterm_cut_hz) {
DTerm = filterApplyPt1(DTerm, &DTermState[axis], pidProfile->dterm_cut_hz);
}
DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5; // 32 bits is needed for calculation
axisPID[axis] = PTerm + ITerm - DTerm;
@ -474,11 +510,22 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
}
PTerm -= ((int32_t)gyroADC[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
}
delta = (gyroADC[axis] - lastGyro[axis]) / 4;
lastGyro[axis] = gyroADC[axis];
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
// Dterm low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz);
}
DTerm = (deltaSum * dynD8[axis]) / 32;
axisPID[axis] = PTerm + ITerm - DTerm;
@ -529,8 +576,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
uint8_t axis;
float ACCDeltaTimeINS, FLOATcycleTime, Mwii3msTimescale;
// MainDptCut = RCconstPI / (float)cfg.maincuthz; // Initialize Cut off frequencies for mainpid D
MainDptCut = RCconstPI / MAIN_CUT_HZ; // maincuthz (default 12Hz, Range 1-50Hz), hardcoded for now
MainDptCut = RCconstPI / constrain(pidProfile->dterm_cut_hz, 1, 50); // maincuthz (default 0 (disabled), Range 1-50Hz)
FLOATcycleTime = (float)constrain(cycleTime, 1, 100000); // 1us - 100ms
ACCDeltaTimeINS = FLOATcycleTime * 0.000001f; // ACCDeltaTimeINS is in seconds now
RCfactor = ACCDeltaTimeINS / (MainDptCut + ACCDeltaTimeINS); // used for pt1 element
@ -585,6 +631,12 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
}
PTerm -= gyroADCQuant * dynP8[axis] * 0.003f;
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
}
delta = (gyroADCQuant - lastGyro[axis]) / ACCDeltaTimeINS;
lastGyro[axis] = gyroADCQuant;
@ -631,6 +683,12 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
}
}
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
}
axisPID[FD_YAW] = PTerm + ITerm;
axisPID[FD_YAW] = lrintf(axisPID[FD_YAW]); // Round up result.
@ -720,6 +778,12 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// -----calculate P component
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
}
// -----calculate I component
// there should be no division before accumulating the error to integrator, because the precision would be reduced.
// Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
@ -743,6 +807,12 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
// Dterm delta low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz);
}
DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
// -----calculate total PID output

5
src/main/flight/pid.h
View file

@ -19,7 +19,6 @@
#define GYRO_I_MAX 256 // Gyro I limiter
#define RCconstPI 0.159154943092f // 0.5f / M_PI;
#define MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz)
#define OLD_YAW 0 // [0/1] 0 = MultiWii 2.3 yaw, 1 = older yaw.
#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter
#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter
@ -63,6 +62,9 @@ typedef struct pidProfile_s {
float H_level;
uint8_t H_sensitivity;
uint16_t yaw_p_limit; // set P term limit (fixed value was 300)
uint8_t dterm_cut_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
uint8_t pterm_cut_hz; // Used for fitlering Pterm noise on noisy frames
uint8_t gyro_cut_hz; // Used for soft gyro filtering
} pidProfile_t;
#define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
@ -75,4 +77,3 @@ void pidSetController(pidControllerType_e type);
void pidResetErrorAngle(void);
void pidResetErrorGyro(void);

3
src/main/io/serial_cli.c
View file

@ -448,6 +448,9 @@ const clivalue_t valueTable[] = {
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], 0, 200 },
{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX },
{ "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, 0, 200 },
{ "pterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pterm_cut_hz, 0, 200 },
{ "gyro_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_cut_hz, 0, 200 },
#ifdef BLACKBOX
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 },

11
src/main/mw.c
View file

@ -72,6 +72,7 @@
#include "flight/failsafe.h"
#include "flight/autotune.h"
#include "flight/navigation.h"
#include "flight/filter.h"
#include "config/runtime_config.h"
@ -740,6 +741,16 @@ void loop(void)
cycleTime = (int32_t)(currentTime - previousTime);
previousTime = currentTime;
// Gyro Low Pass
if (currentProfile->pidProfile.gyro_cut_hz) {
int axis;
static filterStatePt1_t gyroADCState[XYZ_AXIS_COUNT];
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz);
}
}
annexCode();
#if defined(BARO) || defined(SONAR)
haveProcessedAnnexCodeOnce = true;