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

Made filter naming, parameters and state consistent

This commit is contained in:
Martin Budden 2016-06-29 15:02:29 +01:00
parent ebbaeb0976
commit 74d20a276f
8 changed files with 93 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -17,10 +17,8 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include "common/axis.h"
#include "common/filter.h"
#include "common/maths.h"
@ -30,63 +28,70 @@
#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */
#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/
// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dT) {
// PT1 Low Pass filter
void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT)
{
filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
filter->dT = dT;
}
float pt1FilterApply(pt1Filter_t *filter, float input)
{
filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
return filter->state;
}
float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
{
// Pre calculate and store RC
if (!filter->RC) {
filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
filter->dT = dT;
}
filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
return filter->state;
}
/* sets up a biquad Filter */
void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate)
void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate)
{
float sampleRate;
const float sampleRate = 1 / ((float)refreshRate * 0.000001f);
sampleRate = 1 / ((float)refreshRate * 0.000001f);
float omega, sn, cs, alpha;
float a0, a1, a2, b0, b1, b2;
/* setup variables */
omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate;
sn = sinf(omega);
cs = cosf(omega);
// setup variables
const float omega = 2 * M_PI_FLOAT * filterCutFreq / 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);
alpha = sn / (2 * BIQUAD_Q);
const float alpha = sn / (2 * BIQUAD_Q);
b0 = (1 - cs) / 2;
b1 = 1 - cs;
b2 = (1 - cs) / 2;
a0 = 1 + alpha;
a1 = -2 * cs;
a2 = 1 - alpha;
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;
/* precompute the coefficients */
newState->b0 = b0 / a0;
newState->b1 = b1 / a0;
newState->b2 = b2 / a0;
newState->a1 = a1 / a0;
newState->a2 = a2 / a0;
// precompute the coefficients
filter->b0 = b0 / a0;
filter->b1 = b1 / a0;
filter->b2 = b2 / a0;
filter->a1 = a1 / a0;
filter->a2 = a2 / a0;
/* zero initial samples */
newState->d1 = newState->d2 = 1;
// zero initial samples
filter->d1 = filter->d2 = 0;
}
/* Computes a biquad_t filter on a sample */
float applyBiQuadFilter(float sample, biquad_t *state) //direct form 2 transposed
float biquadFilterApply(biquadFilter_t *filter, float input)
{
float result;
result = state->b0 * sample + state->d1;
state->d1 = state->b1 * sample - state->a1 * result + state->d2;
state->d2 = state->b2 * sample - state->a2 * result;
const float result = filter->b0 * input + filter->d1;
filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2;
filter->d2 = filter->b2 * input - filter->a2 * result;
return result;
}

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

@ -17,20 +17,26 @@
#define DELTA_MAX_SAMPLES 12
typedef struct filterStatePt1_s {
typedef struct pt1Filter_s {
float state;
float RC;
float constdT;
} filterStatePt1_t;
float dT;
} pt1Filter_t;
/* this holds the data required to update samples thru a filter */
typedef struct biquad_s {
typedef struct biquadFilter_s {
float b0, b1, b2, a1, a2;
float d1, d2;
} biquad_t;
} biquadFilter_t;
void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate);
float biquadFilterApply(biquadFilter_t *filter, float input);
void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT);
float pt1FilterApply(pt1Filter_t *filter, float input);
float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT);
float applyBiQuadFilter(float sample, biquad_t *state);
float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dt);
int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageState[DELTA_MAX_SAMPLES]);
float filterApplyAveragef(float input, uint8_t averageCount, float averageState[DELTA_MAX_SAMPLES]);
void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate);

1
src/main/flight/imu.c
View File

@ -28,7 +28,6 @@
#include "debug.h"
#include "common/axis.h"
#include "common/filter.h"
#include "drivers/system.h"
#include "drivers/sensor.h"

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

@ -946,7 +946,7 @@ void filterServos(void)
#ifdef USE_SERVOS
static int16_t servoIdx;
static bool servoFilterIsSet;
static biquad_t servoFilterState[MAX_SUPPORTED_SERVOS];
static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS];
#if defined(MIXER_DEBUG)
uint32_t startTime = micros();
@ -955,11 +955,11 @@ void filterServos(void)
if (mixerConfig->servo_lowpass_enable) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
if (!servoFilterIsSet) {
BiQuadNewLpf(mixerConfig->servo_lowpass_freq, &servoFilterState[servoIdx], targetPidLooptime);
biquadFilterInit(&servoFilter[servoIdx], mixerConfig->servo_lowpass_freq, targetPidLooptime);
servoFilterIsSet = true;
}
servo[servoIdx] = lrintf(applyBiQuadFilter((float) servo[servoIdx], &servoFilterState[servoIdx]));
servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx]));
// Sanity check
servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
}

12
src/main/flight/pid.c
View File

@ -112,8 +112,8 @@ float getdT (void)
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static filterStatePt1_t deltaFilterState[3];
static filterStatePt1_t yawFilterState;
static pt1Filter_t deltaFilter[3];
static pt1Filter_t yawFilter;
#ifndef SKIP_PID_LUXFLOAT
static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
@ -205,7 +205,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
//-----calculate D-term
if (axis == YAW) {
if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
axisPID[axis] = lrintf(PTerm + ITerm);
@ -230,7 +230,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
delta *= (1.0f / getdT());
// Filter delta
if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1(delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
DTerm = constrainf(luxDTermScale * delta * (float)pidProfile->D8[axis] * tpaFactor, -300.0f, 300.0f);
@ -344,7 +344,7 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin
//-----calculate D-term
if (axis == YAW) {
if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
axisPID[axis] = PTerm + ITerm;
@ -369,7 +369,7 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5;
// Filter delta
if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], (float)delta, pidProfile->dterm_lpf_hz, getdT());
DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;

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

@ -47,7 +47,6 @@ uint32_t accTargetLooptime;
static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
extern uint16_t InflightcalibratingA;
extern bool AccInflightCalibrationArmed;
extern bool AccInflightCalibrationMeasurementDone;
extern bool AccInflightCalibrationSavetoEEProm;
extern bool AccInflightCalibrationActive;
@ -55,7 +54,7 @@ extern bool AccInflightCalibrationActive;
static flightDynamicsTrims_t *accelerationTrims;
static float accLpfCutHz = 0;
static biquad_t accFilterState[XYZ_AXIS_COUNT];
static biquadFilter_t accFilter[XYZ_AXIS_COUNT];
static bool accFilterInitialised = false;
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
@ -87,9 +86,8 @@ void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims)
void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
{
static int32_t a[3];
int axis;
for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (isOnFirstAccelerationCalibrationCycle())
@ -180,13 +178,12 @@ void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims)
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
{
int16_t accADCRaw[XYZ_AXIS_COUNT];
int axis;
if (!acc.read(accADCRaw)) {
return;
}
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
if (debugMode == DEBUG_ACCELEROMETER) debug[axis] = accADCRaw[axis];
accSmooth[axis] = accADCRaw[axis];
}
@ -194,13 +191,17 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
if (accLpfCutHz) {
if (!accFilterInitialised) {
if (accTargetLooptime) { /* Initialisation needs to happen once sample rate is known */
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(accLpfCutHz, &accFilterState[axis], accTargetLooptime);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime);
}
accFilterInitialised = true;
}
}
if (accFilterInitialised) {
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) accSmooth[axis] = lrintf(applyBiQuadFilter((float) accSmooth[axis], &accFilterState[axis]));
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)accSmooth[axis]));
}
}
}

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

@ -63,20 +63,19 @@ uint16_t batteryAdcToVoltage(uint16_t src)
static void updateBatteryVoltage(void)
{
uint16_t vbatSample;
static biquad_t vbatFilterState;
static bool vbatFilterStateIsSet;
static biquadFilter_t vbatFilter;
static bool vbatFilterIsInitialised;
// store the battery voltage with some other recent battery voltage readings
vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
uint16_t vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample;
if (!vbatFilterStateIsSet) {
BiQuadNewLpf(VBATT_LPF_FREQ, &vbatFilterState, 50000); //50HZ Update
vbatFilterStateIsSet = true;
if (!vbatFilterIsInitialised) {
biquadFilterInit(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update
vbatFilterIsInitialised = true;
}
vbatSample = applyBiQuadFilter(vbatSample, &vbatFilterState);
vbatSample = biquadFilterApply(&vbatFilter, vbatSample);
vbat = batteryAdcToVoltage(vbatSample);
if (debugMode == DEBUG_BATTERY) debug[1] = vbat;

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

@ -43,7 +43,7 @@ float gyroADCf[XYZ_AXIS_COUNT];
static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
static const gyroConfig_t *gyroConfig;
static biquad_t gyroFilterState[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilter[XYZ_AXIS_COUNT];
static uint8_t gyroSoftLpfHz;
static uint16_t calibratingG = 0;
@ -57,7 +57,7 @@ void gyroInit(void)
{
if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known
for (int axis = 0; axis < 3; axis++) {
BiQuadNewLpf(gyroSoftLpfHz, &gyroFilterState[axis], gyro.targetLooptime);
biquadFilterInit(&gyroFilter[axis], gyroSoftLpfHz, gyro.targetLooptime);
}
}
}
@ -157,7 +157,7 @@ void gyroUpdate(void)
if (gyroSoftLpfHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADCf[axis] = applyBiQuadFilter((float)gyroADC[axis], &gyroFilterState[axis]);
gyroADCf[axis] = biquadFilterApply(&gyroFilter[axis], (float)gyroADC[axis]);
gyroADC[axis] = lrintf(gyroADCf[axis]);
}
} else {