Made filter naming, parameters and state consistent
This commit is contained in:
Martin Budden
parent
ebbaeb0976
commit
74d20a276f
|
|
@ -17,76 +17,81 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/filter.h"
|
||||
#include "common/maths.h"
|
||||
|
||||
#define M_LN2_FLOAT 0.69314718055994530942f
|
||||
#define M_PI_FLOAT 3.14159265358979323846f
|
||||
#define M_LN2_FLOAT 0.69314718055994530942f
|
||||
#define M_PI_FLOAT 3.14159265358979323846f
|
||||
|
||||
#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
|
||||
|
||||
// Pre calculate and store RC
|
||||
if (!filter->RC) {
|
||||
filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
|
||||
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 + 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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -17,20 +17,26 @@
|
|||
|
||||
#define DELTA_MAX_SAMPLES 12
|
||||
|
||||
typedef struct filterStatePt1_s {
|
||||
float state;
|
||||
float RC;
|
||||
float constdT;
|
||||
} filterStatePt1_t;
|
||||
typedef struct pt1Filter_s {
|
||||
float state;
|
||||
float RC;
|
||||
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);
|
||||
|
||||
|
|
|
|||
|
|
@ -28,7 +28,6 @@
|
|||
#include "debug.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/filter.h"
|
||||
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/sensor.h"
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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())
|
||||
|
|
@ -179,14 +177,13 @@ void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims)
|
|||
|
||||
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
|
||||
{
|
||||
int16_t accADCRaw[XYZ_AXIS_COUNT];
|
||||
int axis;
|
||||
int16_t accADCRaw[XYZ_AXIS_COUNT];
|
||||
|
||||
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]));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
Loading…
Reference in New Issue