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

Lead-Lag Compensator (#12730)

Browse source 
* Implement Lead-Lag-Compensator

* Refactoring / Clean up

* Remove trailing whitespaces
This commit is contained in:
Jan Post 2023-05-03 10:01:36 +02:00 • committed by GitHub
parent 405627dec2
commit 460e4d00fe
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
7 changed files with 151 additions and 92 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -19,7 +19,6 @@
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
@ -131,30 +130,7 @@ FAST_CODE float pt3FilterApply(pt3Filter_t *filter, float input)
}
// Slew filter with limit
void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold)
{
filter->state = 0.0f;
filter->slewLimit = slewLimit;
filter->threshold = threshold;
}
FAST_CODE float slewFilterApply(slewFilter_t *filter, float input)
{
if (filter->state >= filter->threshold) {
if (input >= filter->state - filter->slewLimit) {
filter->state = input;
}
} else if (filter->state <= -filter->threshold) {
if (input <= filter->state + filter->slewLimit) {
filter->state = input;
}
} else {
filter->state = input;
}
return filter->state;
}
// Biquad filter
// get notch filter Q given center frequency (f0) and lower cutoff frequency (f1)
// Q = f0 / (f2 - f1) ; f2 = f0^2 / f1
@ -268,6 +244,76 @@ FAST_CODE float biquadFilterApply(biquadFilter_t *filter, float input)
return result;
}
// Phase Compensator (Lead-Lag-Compensator)
void phaseCompInit(phaseComp_t *filter, const float centerFreqHz, const float centerPhaseDeg, const uint32_t looptimeUs)
{
phaseCompUpdate(filter, centerFreqHz, centerPhaseDeg, looptimeUs);
filter->x1 = 0.0f;
filter->y1 = 0.0f;
}
FAST_CODE void phaseCompUpdate(phaseComp_t *filter, const float centerFreqHz, const float centerPhaseDeg, const uint32_t looptimeUs)
{
const float omega = 2.0f * M_PIf * centerFreqHz * looptimeUs * 1e-6f;
const float sn = sin_approx(centerPhaseDeg * RAD);
const float gain = (1 + sn) / (1 - sn);
const float alpha = (12 - sq(omega)) / (6 * omega * sqrtf(gain)); // approximate prewarping (series expansion)
filter->b0 = 1 + alpha * gain;
filter->b1 = 2 - filter->b0;
filter->a1 = 1 - alpha;
const float a0 = 1 / (1 + alpha);
filter->b0 *= a0;
filter->b1 *= a0;
filter->a1 *= a0;
}
FAST_CODE float phaseCompApply(phaseComp_t *filter, const float input)
{
// compute result
const float result = filter->b0 * input + filter->b1 * filter->x1 - filter->a1 * filter->y1;
// shift input to x1 and result to y1
filter->x1 = input;
filter->y1 = result;
return result;
}
// Slew filter with limit
void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold)
{
filter->state = 0.0f;
filter->slewLimit = slewLimit;
filter->threshold = threshold;
}
FAST_CODE float slewFilterApply(slewFilter_t *filter, float input)
{
if (filter->state >= filter->threshold) {
if (input >= filter->state - filter->slewLimit) {
filter->state = input;
}
} else if (filter->state <= -filter->threshold) {
if (input <= filter->state + filter->slewLimit) {
filter->state = input;
}
} else {
filter->state = input;
}
return filter->state;
}
// Moving average
void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, float *buf)
{
filter->movingWindowIndex = 0;
@ -293,8 +339,16 @@ FAST_CODE float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float i
return filter->movingSum / denom;
}
// Simple fixed-point lowpass filter based on integer math
void simpleLPFilterInit(simpleLowpassFilter_t *filter, int32_t beta, int32_t fpShift)
{
filter->fp = 0;
filter->beta = beta;
filter->fpShift = fpShift;
}
int32_t simpleLPFilterUpdate(simpleLowpassFilter_t *filter, int32_t newVal)
{
filter->fp = (filter->fp << filter->beta) - filter->fp;
@ -304,11 +358,13 @@ int32_t simpleLPFilterUpdate(simpleLowpassFilter_t *filter, int32_t newVal)
return result;
}
void simpleLPFilterInit(simpleLowpassFilter_t *filter, int32_t beta, int32_t fpShift)
// Mean accumulator
void meanAccumulatorInit(meanAccumulator_t *filter)
{
filter->fp = 0;
filter->beta = beta;
filter->fpShift = fpShift;
filter->accumulator = 0;
filter->count = 0;
}
void meanAccumulatorAdd(meanAccumulator_t *filter, const int8_t newVal)
@ -326,9 +382,3 @@ int8_t meanAccumulatorCalc(meanAccumulator_t *filter, const int8_t defaultValue)
}
return defaultValue;
}
void meanAccumulatorInit(meanAccumulator_t *filter)
{
filter->accumulator = 0;
filter->count = 0;
}

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

@ -19,10 +19,26 @@
*/
#pragma once
#include <stdbool.h>
#include <stdint.h>
struct filter_s;
typedef struct filter_s filter_t;
typedef float (*filterApplyFnPtr)(filter_t *filter, float input);
typedef enum {
FILTER_PT1 = 0,
FILTER_BIQUAD,
FILTER_PT2,
FILTER_PT3,
} lowpassFilterType_e;
typedef enum {
FILTER_LPF, // 2nd order Butterworth section
FILTER_NOTCH,
FILTER_BPF,
} biquadFilterType_e;
typedef struct pt1Filter_s {
float state;
@ -42,12 +58,6 @@ typedef struct pt3Filter_s {
float k;
} pt3Filter_t;
typedef struct slewFilter_s {
float state;
float slewLimit;
float threshold;
} slewFilter_t;
/* this holds the data required to update samples thru a filter */
typedef struct biquadFilter_s {
float b0, b1, b2, a1, a2;
@ -55,6 +65,17 @@ typedef struct biquadFilter_s {
float weight;
} biquadFilter_t;
typedef struct phaseComp_s {
float b0, b1, a1;
float x1, y1;
} phaseComp_t;
typedef struct slewFilter_s {
float state;
float slewLimit;
float threshold;
} slewFilter_t;
typedef struct laggedMovingAverage_s {
uint16_t movingWindowIndex;
uint16_t windowSize;
@ -63,36 +84,19 @@ typedef struct laggedMovingAverage_s {
bool primed;
} laggedMovingAverage_t;
typedef enum {
FILTER_PT1 = 0,
FILTER_BIQUAD,
FILTER_PT2,
FILTER_PT3,
} lowpassFilterType_e;
typedef struct simpleLowpassFilter_s {
int32_t fp;
int32_t beta;
int32_t fpShift;
} simpleLowpassFilter_t;
typedef enum {
FILTER_LPF, // 2nd order Butterworth section
FILTER_NOTCH,
FILTER_BPF,
} biquadFilterType_e;
typedef float (*filterApplyFnPtr)(filter_t *filter, float input);
typedef struct meanAccumulator_s {
int32_t accumulator;
int32_t count;
} meanAccumulator_t;
float nullFilterApply(filter_t *filter, float input);
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight);
void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight);
void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
float biquadFilterApplyDF1Weighted(biquadFilter_t *filter, float input);
float biquadFilterApply(biquadFilter_t *filter, float input);
float filterGetNotchQ(float centerFreq, float cutoffFreq);
void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, float *buf);
float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input);
float pt1FilterGain(float f_cut, float dT);
void pt1FilterInit(pt1Filter_t *filter, float k);
void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k);
@ -108,23 +112,29 @@ void pt3FilterInit(pt3Filter_t *filter, float k);
void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k);
float pt3FilterApply(pt3Filter_t *filter, float input);
float filterGetNotchQ(float centerFreq, float cutoffFreq);
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight);
void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight);
void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
float biquadFilterApplyDF1Weighted(biquadFilter_t *filter, float input);
float biquadFilterApply(biquadFilter_t *filter, float input);
void phaseCompInit(phaseComp_t *filter, const float centerFreq, const float centerPhase, const uint32_t looptimeUs);
void phaseCompUpdate(phaseComp_t *filter, const float centerFreq, const float centerPhase, const uint32_t looptimeUs);
float phaseCompApply(phaseComp_t *filter, const float input);
void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold);
float slewFilterApply(slewFilter_t *filter, float input);
typedef struct simpleLowpassFilter_s {
int32_t fp;
int32_t beta;
int32_t fpShift;
} simpleLowpassFilter_t;
void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, float *buf);
float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input);
int32_t simpleLPFilterUpdate(simpleLowpassFilter_t *filter, int32_t newVal);
void simpleLPFilterInit(simpleLowpassFilter_t *filter, int32_t beta, int32_t fpShift);
int32_t simpleLPFilterUpdate(simpleLowpassFilter_t *filter, int32_t newVal);
typedef struct meanAccumulator_s {
int32_t accumulator;
int32_t count;
} meanAccumulator_t;
void meanAccumulatorInit(meanAccumulator_t *filter);
void meanAccumulatorAdd(meanAccumulator_t *filter, const int8_t newVal);
int8_t meanAccumulatorCalc(meanAccumulator_t *filter, const int8_t defaultValue);
void meanAccumulatorInit(meanAccumulator_t *filter);

1
src/main/flight/pid_init.c
View file

@ -432,4 +432,3 @@ void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)
memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t));
}
}