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

Merge pull request #1028 from borisbstyle/dterm_filter

Browse source 
Selective Software Filtering (Gyro, Dterm, Pterm)

Thanks for the code boris and thanks to everyone else for their testing and feedback
This commit is contained in:
Dominic Clifton 2015-06-29 16:00:49 +01:00
commit e48e50512e
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;
}

43
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:
mpuLowPassFilter = BITS_DLPF_CFG_256HZ;
break;
case 188:
mpuLowPassFilter = BITS_DLPF_CFG_188HZ;
break;
case 98:
mpuLowPassFilter = BITS_DLPF_CFG_98HZ;
break;
default:
case 42:
mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
break;
case 20:
mpuLowPassFilter = BITS_DLPF_CFG_20HZ;
break;
case 10:
mpuLowPassFilter = BITS_DLPF_CFG_10HZ;
break;
case 5:
mpuLowPassFilter = BITS_DLPF_CFG_5HZ;
break;
case 0:
mpuLowPassFilter = BITS_DLPF_CFG_2100HZ_NOLPF;
break;
}
if (lpf == 256)
mpuLowPassFilter = BITS_DLPF_CFG_256HZ;
else if (lpf >= 188)
mpuLowPassFilter = BITS_DLPF_CFG_188HZ;
else if (lpf >= 98)
mpuLowPassFilter = BITS_DLPF_CFG_98HZ;
else if (lpf >= 42)
mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
else if (lpf >= 20)
mpuLowPassFilter = BITS_DLPF_CFG_20HZ;
else if (lpf >= 10)
mpuLowPassFilter = BITS_DLPF_CFG_10HZ;
else if (lpf > 0)
mpuLowPassFilter = BITS_DLPF_CFG_5HZ;
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;