rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #684 from rav-rav/notch 

Smoother filtering (additional notch filter)
This commit is contained in:
borisbstyle 2016-07-16 10:30:47 +02:00 committed by GitHub
parent c21fcaf2b4 86626acbdf
commit 4fff6c89bf
12 changed files with 71 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/blackbox/blackbox.c
View File

@ -1219,6 +1219,8 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", masterConfig.gyro_lpf);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d", (int)(masterConfig.gyro_soft_notch_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_q:%d", (int)(masterConfig.gyro_soft_notch_q * 10.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", masterConfig.acc_hardware);
BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.baro_hardware);

39
src/main/common/filter.c
View File

@ -56,24 +56,39 @@ float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
}
/* sets up a biquad Filter */
void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate)
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
{
biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF);
}
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, filterType_e filterType)
{
const float sampleRate = 1 / ((float)refreshRate * 0.000001f);
// setup variables
const float omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate;
const float sampleRate = 1 / ((float)refreshRate * 0.000001f);
const float omega = 2 * M_PI_FLOAT * filterFreq / sampleRate;
const float sn = sinf(omega);
const float cs = cosf(omega);
//this is wrong, should be hyperbolic sine
//alpha = sn * sinf(M_LN2_FLOAT /2 * BIQUAD_BANDWIDTH * omega /sn);
const float alpha = sn / (2 * BIQUAD_Q);
const float b0 = (1 - cs) / 2;
const float b1 = 1 - cs;
const float b2 = (1 - cs) / 2;
const float a0 = 1 + alpha;
const float a1 = -2 * cs;
const float a2 = 1 - alpha;
float b0, b1, b2, a0, a1, a2;
switch (filterType) {
case FILTER_LPF:
b0 = (1 - cs) / 2;
b1 = 1 - cs;
b2 = (1 - cs) / 2;
a0 = 1 + alpha;
a1 = -2 * cs;
a2 = 1 - alpha;
break;
case FILTER_NOTCH:
b0 = 1;
b1 = -2 * cs;
b2 = 1;
a0 = 1 + alpha;
a1 = -2 * cs;
a2 = 1 - alpha;
break;
}
// precompute the coefficients
filter->b0 = b0 / a0;

7
src/main/common/filter.h
View File

@ -29,8 +29,13 @@ typedef struct biquadFilter_s {
float d1, d2;
} biquadFilter_t;
typedef enum {
FILTER_LPF,
FILTER_NOTCH
} filterType_e;
void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate);
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, filterType_e filterType);
float biquadFilterApply(biquadFilter_t *filter, float input);
void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT);

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

@ -456,6 +456,8 @@ static void resetConf(void)
masterConfig.gyro_sync_denom = 4;
#endif
masterConfig.gyro_soft_lpf_hz = 100;
masterConfig.gyro_soft_notch_hz = 0;
masterConfig.gyro_soft_notch_q = 5;
masterConfig.pid_process_denom = 2;
@ -717,7 +719,7 @@ void activateConfig(void)
&currentProfile->pidProfile
);
gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz);
gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz, masterConfig.gyro_soft_notch_q);
#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);

4
src/main/config/config_master.h
View File

@ -58,7 +58,9 @@ typedef struct master_t {
uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes.
uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint8_t gyro_sync_denom; // Gyro sample divider
uint8_t gyro_soft_lpf_hz; // Biqyad gyro lpf hz
uint8_t gyro_soft_lpf_hz; // Biquad gyro lpf hz
uint16_t gyro_soft_notch_hz; // Biquad gyro notch hz
uint8_t gyro_soft_notch_q; // Biquad gyro notch quality
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t dcm_ki; // DCM filter integral gain ( x 10000)

1
src/main/debug.h
View File

@ -50,5 +50,6 @@ typedef enum {
DEBUG_MIXER,
DEBUG_AIRMODE,
DEBUG_PIDLOOP,
DEBUG_NOTCH,
DEBUG_COUNT
} debugType_e;

2
src/main/flight/mixer.c
View File

@ -956,7 +956,7 @@ void filterServos(void)
if (mixerConfig->servo_lowpass_enable) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
if (!servoFilterIsSet) {
biquadFilterInit(&servoFilter[servoIdx], mixerConfig->servo_lowpass_freq, targetPidLooptime);
biquadFilterInitLPF(&servoFilter[servoIdx], mixerConfig->servo_lowpass_freq, targetPidLooptime);
servoFilterIsSet = true;
}

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

@ -453,6 +453,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
"MIXER",
"AIRMODE",
"PIDLOOP",
"NOTCH",
};
#ifdef OSD
static const char * const lookupTableOsdType[] = {
@ -698,6 +699,8 @@ const clivalue_t valueTable[] = {
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } },
{ "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } },
{ "gyro_notch_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz, .config.minmax = { 0, 500 } },
{ "gyro_notch_q", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_notch_q, .config.minmax = { 1, 100 } },
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } },
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } },
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } },

2
src/main/sensors/acceleration.c
View File

@ -190,7 +190,7 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
if (!accFilterInitialised) {
if (accTargetLooptime) { /* Initialisation needs to happen once sample rate is known */
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime);
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime);
}
accFilterInitialised = true;
}

2
src/main/sensors/battery.c
View File

@ -70,7 +70,7 @@ static void updateBatteryVoltage(void)
if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample;
if (!vbatFilterIsInitialised) {
biquadFilterInit(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update
biquadFilterInitLPF(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update
vbatFilterIsInitialised = true;
}
vbatSample = biquadFilterApply(&vbatFilter, vbatSample);

25
src/main/sensors/gyro.c
View File

@ -27,6 +27,7 @@
#include "common/filter.h"
#include "drivers/sensor.h"
#include "drivers/system.h"
#include "drivers/accgyro.h"
#include "io/beeper.h"
@ -44,21 +45,27 @@ float gyroADCf[XYZ_AXIS_COUNT];
static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
static const gyroConfig_t *gyroConfig;
static biquadFilter_t gyroFilter[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilterNotch[XYZ_AXIS_COUNT];
static uint16_t gyroSoftNotchHz;
static uint8_t gyroSoftNotchQ;
static uint8_t gyroSoftLpfHz;
static uint16_t calibratingG = 0;
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz)
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q)
{
gyroConfig = gyroConfigToUse;
gyroSoftLpfHz = gyro_soft_lpf_hz;
gyroSoftNotchHz = gyro_soft_notch_hz;
gyroSoftNotchQ = gyro_soft_notch_q;
}
void gyroInit(void)
{
if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroFilter[axis], gyroSoftLpfHz, gyro.targetLooptime);
biquadFilterInit(&gyroFilterNotch[axis], gyroSoftNotchHz, gyro.targetLooptime, ((float) gyroSoftNotchQ) / 10, FILTER_NOTCH);
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
}
}
}
@ -158,7 +165,17 @@ void gyroUpdate(void)
if (gyroSoftLpfHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADCf[axis] = biquadFilterApply(&gyroFilter[axis], (float)gyroADC[axis]);
float sample = (float) gyroADC[axis];
if (gyroSoftNotchHz) {
sample = biquadFilterApply(&gyroFilterNotch[axis], sample);
}
if (debugMode == DEBUG_NOTCH && axis < 2){
debug[axis*2 + 0] = gyroADC[axis];
debug[axis*2 + 1] = lrintf(sample);
}
gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], sample);;
gyroADC[axis] = lrintf(gyroADCf[axis]);
}
} else {

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

@ -40,7 +40,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 gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz);
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q);
void gyroSetCalibrationCycles(void);
void gyroInit(void);
void gyroUpdate(void);