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

2
src/main/cli/settings.c
View file

@ -1142,7 +1142,7 @@ const clivalue_t valueTable[] = {
{ PARAM_NAME_HORIZON_LIMIT_STICKS, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].D) },
{ PARAM_NAME_HORIZON_LIMIT_DEGREES, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_limit_degrees) },
{ PARAM_NAME_HORIZON_IGNORE_STICKS, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_ignore_sticks) },
{ PARAM_NAME_HORIZON_DELAY_MS, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 5000 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_delay_ms) },
{ PARAM_NAME_HORIZON_DELAY_MS, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 5000 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_delay_ms) },
#if defined(USE_ABSOLUTE_CONTROL)
{ PARAM_NAME_ABS_CONTROL_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_gain) },

122
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;
@ -290,11 +336,19 @@ FAST_CODE float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float i
}
const uint16_t denom = filter->primed ? filter->windowSize : filter->movingWindowIndex;
return filter->movingSum / denom;
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);

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

@ -123,7 +123,7 @@ typedef struct dynNotch_s {
int maxCenterFreq;
float centerFreq[XYZ_AXIS_COUNT][DYN_NOTCH_COUNT_MAX];
timeUs_t looptimeUs;
biquadFilter_t notch[XYZ_AXIS_COUNT][DYN_NOTCH_COUNT_MAX];
@ -261,11 +261,11 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void)
DEBUG_SET(DEBUG_FFT_TIME, 0, state.step);
switch (state.step) {
case STEP_WINDOW: // 4.1us (3-6us) @ F722
{
sdftWinSq(&sdft[state.axis], sdftData);
// Get total vibrational power in dyn notch range for noise floor estimate in STEP_CALC_FREQUENCIES
sdftNoiseThreshold = 0.0f;
for (int bin = sdftStartBin; bin <= sdftEndBin; bin++) {
@ -359,7 +359,7 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void)
// Convert bin to frequency: freq = bin * binResoultion (bin 0 is 0Hz)
const float centerFreq = constrainf(meanBin * sdftResolutionHz, dynNotch.minHz, dynNotch.maxHz);
// PT1 style smoothing moves notch center freqs rapidly towards big peaks and slowly away, up to 10x faster
// PT1 style smoothing moves notch center freqs rapidly towards big peaks and slowly away, up to 10x faster
const float cutoffMult = constrainf(peaks[p].value / sdftNoiseThreshold, 1.0f, 10.0f);
const float gain = pt1FilterGain(DYN_NOTCH_SMOOTH_HZ * cutoffMult, pt1LooptimeS); // dynamic PT1 k value
@ -403,7 +403,7 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void)
state.step = (state.step + 1) % STEP_COUNT;
}
FAST_CODE float dynNotchFilter(const int axis, float value)
FAST_CODE float dynNotchFilter(const int axis, float value)
{
for (int p = 0; p < dynNotch.count; p++) {
value = biquadFilterApplyDF1(&dynNotch.notch[axis][p], value);

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

@ -91,7 +91,7 @@ pt1Filter_t throttleLpf;
PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 3);
#if !defined(DEFAULT_PID_PROCESS_DENOM)
#if defined(STM32F411xE)
#if defined(STM32F411xE)
#define DEFAULT_PID_PROCESS_DENOM 2
#else
#define DEFAULT_PID_PROCESS_DENOM 1
@ -298,7 +298,7 @@ void pidUpdateAntiGravityThrottleFilter(float throttle)
static float previousThrottle = 0.0f;
const float throttleInv = 1.0f - throttle;
float throttleDerivative = fabsf(throttle - previousThrottle) * pidRuntime.pidFrequency;
DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(throttleDerivative * 100));
DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(throttleDerivative * 100));
throttleDerivative *= throttleInv * throttleInv;
// generally focus on the low throttle period
if (throttle > previousThrottle) {
@ -310,7 +310,7 @@ void pidUpdateAntiGravityThrottleFilter(float throttle)
// lower cutoff suppresses peaks relative to troughs and prolongs the effects
// PT2 smoothing of throttle derivative.
// 6 is a typical value for the peak boost factor with default cutoff of 6Hz
DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(throttleDerivative * 100));
DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(throttleDerivative * 100));
pidRuntime.antiGravityThrottleD = throttleDerivative;
}
@ -911,7 +911,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
if (levelMode == LEVEL_MODE_RP) {
// if earth referencing is requested, attenuate yaw axis setpoint when pitched or rolled
// and send yawSetpoint to Angle code to modulate pitch and roll
// code cost is 107 cycles when earthRef enabled, 20 otherwise, nearly all in cos_approx
// code cost is 107 cycles when earthRef enabled, 20 otherwise, nearly all in cos_approx
if (pidRuntime.angleEarthRef) {
pidRuntime.angleYawSetpoint = currentPidSetpoint;
float maxAngleTargetAbs = pidRuntime.angleEarthRef * fmaxf( fabsf(pidRuntime.angleTarget[FD_ROLL]), fabsf(pidRuntime.angleTarget[FD_PITCH]) );

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));
}
}

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

@ -108,7 +108,7 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
gyroConfig->gyroMovementCalibrationThreshold = 48;
gyroConfig->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL;
gyroConfig->gyro_lpf1_type = FILTER_PT1;
gyroConfig->gyro_lpf1_static_hz = GYRO_LPF1_DYN_MIN_HZ_DEFAULT;
gyroConfig->gyro_lpf1_static_hz = GYRO_LPF1_DYN_MIN_HZ_DEFAULT;
// NOTE: dynamic lpf is enabled by default so this setting is actually
// overridden and the static lowpass 1 is disabled. We can't set this
// value to 0 otherwise Configurator versions 10.4 and earlier will also