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

Gyro Soft LPF Rework // Filter rework

Browse source 
remove 3 point average

lux optimize
This commit is contained in:
borisbstyle 2015-11-17 13:55:45 +01:00
parent 28944852a8
commit 054868e0a4
8 changed files with 64 additions and 31 deletions
Download patch file Download diff file Expand all files Collapse all files

36
src/main/common/filter.c
View file

@ -10,8 +10,10 @@
#include <stdlib.h>
#include <math.h>
#include "common/axis.h"
#include "common/filter.h"
#include "common/axis.h"
#include "common/maths.h"
// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
@ -27,23 +29,35 @@ float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float
return filter->state;
}
// 7 Tap FIR filter as described here:
// Thanks to Qcopter
void filterApply7TapFIR(int16_t data[]) {
int16_t FIRcoeff[7] = { 12, 23, 40, 51, 52, 40, 38 }; // TODO - More coefficients needed. Now fixed to 1khz
static int16_t gyro_delay[3][7] = { {0}, {0}, {0} };
/**
* Typical quadcopter motor noise frequency (at 50% throttle):
* 450-sized, 920kv, 9.4x4.3 props, 3S : 4622rpm = 77Hz
* 250-sized, 2300kv, 5x4.5 props, 4S : 14139rpm = 235Hz
*/
static int8_t gyroFIRCoeff_1000[3][9] = { { 0, 0, 12, 23, 40, 51, 52, 40, 38 }, // 1khz; group delay 2.5ms; -0.5db = 32Hz ; -1db = 45Hz; -5db = 97Hz; -10db = 132Hz
{ 18, 30, 42, 46, 40, 34, 22, 8, 8}, // 1khz; group delay 3ms; -0.5db = 18Hz ; -1db = 33Hz; -5db = 81Hz; -10db = 113Hz
{ 18, 12, 28, 40, 44, 40, 32, 22, 20} }; // 1khz; group delay 4ms; -0.5db = 23Hz ; -1db = 35Hz; -5db = 75Hz; -10db = 103Hz
int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level)
{
return gyroFIRCoeff_1000[filter_level];
}
// 9 Tap FIR filter as described here:
// Thanks to Qcopter & BorisB & DigitalEntity
void filterApply9TapFIR(int16_t data[3], int16_t state[3][9], int8_t coeff[9])
{
int32_t FIRsum;
int axis, i;
// 7 tap FIR, <-20dB at >170Hz with looptime 1ms, groupdelay = 2.5ms
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
FIRsum = 0;
for (i = 0; i <= 5; i++) {
gyro_delay[axis][i] = gyro_delay[axis][i + 1];
FIRsum += gyro_delay[axis][i] * FIRcoeff[i];
for (i = 0; i <= 7; i++) {
state[axis][i] = state[axis][i + 1];
FIRsum += state[axis][i] * (int16_t)coeff[i];
}
gyro_delay[axis][6] = data[axis];
FIRsum += gyro_delay[axis][6] * FIRcoeff[6];
state[axis][8] = data[axis];
FIRsum += state[axis][8] * coeff[8];
data[axis] = FIRsum / 256;
}
}

4
src/main/common/filter.h
View file

@ -13,5 +13,5 @@ typedef struct filterStatePt1_s {
} filterStatePt1_t;
float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
void filterApply7TapFIR(int16_t data[]);
int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level);
void filterApply9TapFIR(int16_t data[3], int16_t state[3][9], int8_t coeff[9]);

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

@ -26,6 +26,7 @@
#include "common/color.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/filter.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
@ -174,6 +175,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->I8[PIDVEL] = 45;
pidProfile->D8[PIDVEL] = 1;
pidProfile->gyro_soft_lpf = 0; // LOW filtering by default
pidProfile->dterm_cut_hz = 40;
pidProfile->yaw_pterm_cut_hz = 50;
@ -691,7 +693,7 @@ void activateConfig(void)
&currentProfile->pidProfile
);
useGyroConfig(&masterConfig.gyroConfig);
useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable(currentProfile->pidProfile.gyro_soft_lpf));
#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);

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

@ -142,11 +142,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
} else {
// calculate error and limit the angle to the max inclination
#ifdef GPS
errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
errorAngle = (constrainf(((float)rcCommand[axis] * ((float)max_angle_inclination / 500.0f)) + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f;
#else
errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
errorAngle = (constrainf((float)rcCommand[axis] * ((float)max_angle_inclination / 500.0f), -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f;
#endif
if (FLIGHT_MODE(ANGLE_MODE)) {
@ -341,7 +341,7 @@ void pidSetController(pidControllerType_e type)
{
switch (type) {
default:
case PID_CONTROLLER_REWRITE:
case PID_CONTROLLER_MWREWRITE:
pid_controller = pidRewrite;
break;
case PID_CONTROLLER_LUX_FLOAT:

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

@ -18,6 +18,8 @@
#pragma once
#define GYRO_I_MAX 256 // Gyro I limiter
#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter
#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter
typedef enum {
PIDROLL,
@ -34,7 +36,7 @@ typedef enum {
} pidIndex_e;
typedef enum {
PID_CONTROLLER_REWRITE = 1,
PID_CONTROLLER_MWREWRITE,
PID_CONTROLLER_LUX_FLOAT,
PID_COUNT
} pidControllerType_e;
@ -42,7 +44,7 @@ typedef enum {
#define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2)
typedef struct pidProfile_s {
uint8_t pidController; // 1 = rewrite, 2 = luxfloat
uint8_t pidController; // 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 2 = Luggi09s new baseflight pid
uint8_t P8[PID_ITEM_COUNT];
uint8_t I8[PID_ITEM_COUNT];
@ -55,8 +57,10 @@ 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 yaw_pterm_cut_hz; // Yaw P low pass filter for pterm. very usefull on noisy setups
uint8_t yaw_pterm_cut_hz; // Used for filering Pterm noise on noisy frames
uint8_t gyro_soft_lpf; // Gyro FIR filter
#ifdef GTUNE
uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune

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

@ -350,6 +350,10 @@ static const char * const lookupTableSerialRX[] = {
"XB-B-RJ01"
};
static const char * const lookupTableGyroFilter[] = {
"OFF", "LOW", "MEDIUM", "HIGH"
};
typedef struct lookupTableEntry_s {
const char * const *values;
@ -367,7 +371,8 @@ typedef enum {
TABLE_CURRENT_SENSOR,
TABLE_GIMBAL_MODE,
TABLE_PID_CONTROLLER,
TABLE_SERIAL_RX
TABLE_SERIAL_RX,
TABLE_GYRO_FILTER,
} lookupTableIndex_e;
static const lookupTableEntry_t lookupTables[] = {
@ -381,7 +386,8 @@ static const lookupTableEntry_t lookupTables[] = {
{ lookupTableCurrentSensor, sizeof(lookupTableCurrentSensor) / sizeof(char *) },
{ lookupTableGimbalMode, sizeof(lookupTableGimbalMode) / sizeof(char *) },
{ lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) },
{ lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) }
{ lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) },
{ lookupTableGyroFilter, sizeof(lookupTableGyroFilter) / sizeof(char *) }
};
#define VALUE_TYPE_OFFSET 0
@ -626,8 +632,9 @@ const clivalue_t valueTable[] = {
{ "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], .config.minmax = { 0, 200 } },
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0, 200 } },
{ "yaw_pterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_pterm_cut_hz, .config.minmax = { 0, 200 } },
{ "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, .config.minmax = { 0, 200 } },
{ "gyro_soft_lpf", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.gyro_soft_lpf, .config.lookup = { TABLE_GYRO_FILTER } },
{ "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, .config.minmax = {0, 200 } },
{ "yaw_pterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_pterm_cut_hz, .config.minmax = {0, 200 } },
#ifdef GTUNE
{ "gtune_loP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], .config.minmax = { 10, 200 } },

12
src/main/sensors/gyro.c
View file

@ -38,13 +38,16 @@ int16_t gyroADC[XYZ_AXIS_COUNT];
int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
static gyroConfig_t *gyroConfig;
static int8_t * gyroFIRTable = 0L;
static int16_t gyroFIRState[3][9];
gyro_t gyro; // gyro access functions
sensor_align_e gyroAlign = 0;
void useGyroConfig(gyroConfig_t *gyroConfigToUse)
void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t * filterTableToUse)
{
gyroConfig = gyroConfigToUse;
gyroFIRTable = filterTableToUse;
}
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
@ -121,9 +124,12 @@ void gyroUpdate(void)
if (!gyro.read(gyroADC)) {
return;
}
alignSensors(gyroADC, gyroADC, gyroAlign);
filterApply7TapFIR(gyroADC); // Apply filter to gyro
if (gyroFIRTable) {
filterApply9TapFIR(gyroADC, gyroFIRState, gyroFIRTable);
}
alignSensors(gyroADC, gyroADC, gyroAlign);
if (!isGyroCalibrationComplete()) {
performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold);

2
src/main/sensors/gyro.h
View file

@ -41,7 +41,7 @@ typedef struct gyroConfig_s {
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
} gyroConfig_t;
void useGyroConfig(gyroConfig_t *gyroConfigToUse);
void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t * filterTableToUse);
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void gyroUpdate(void);
bool isGyroCalibrationComplete(void);