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

Merge pull request #1641 from martinbudden/bf_pid_tidy2 

Use function pointers to tidy PID filter handling
This commit is contained in:
borisbstyle 2016-11-25 23:55:58 +01:00 committed by GitHub
parent 28caf43099 032d9354ed
commit 0d53911257
1 changed files with 90 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -21,6 +21,7 @@
#include <platform.h>
#include "build/build_config.h"
#include "build/debug.h"
#include "common/axis.h"
@ -74,33 +75,77 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState)
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
pt1Filter_t deltaFilter[3];
pt1Filter_t yawFilter;
biquadFilter_t dtermFilterLpf[3];
biquadFilter_t dtermFilterNotch[3];
bool dtermNotchInitialised;
bool dtermBiquadLpfInitialised;
firFilterDenoise_t dtermDenoisingState[3];
static filterApplyFnPtr dtermNotchFilterApplyFn;
static void *dtermFilterNotch[2];
static filterApplyFnPtr dtermLpfApplyFn;
static void *dtermFilterLpf[2];
static filterApplyFnPtr ptermYawFilterApplyFn;
static void *ptermYawFilter;
static void pidInitFilters(const pidProfile_t *pidProfile)
{
static uint8_t lowpassFilterType;
static bool initialized = false; // !!TODO - remove this temporary measure once filter lazy initialization is removed
static biquadFilter_t biquadFilterNotch[2];
static pt1Filter_t pt1Filter[2];
static biquadFilter_t biquadFilter[2];
static firFilterDenoise_t denoisingFilter[2];
static pt1Filter_t pt1FilterYaw;
if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) {
float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff);
for (int axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH);
dtermNotchInitialised = true;
if (initialized) {
return;
}
if ((pidProfile->dterm_filter_type != lowpassFilterType) && pidProfile->dterm_lpf_hz) {
if (pidProfile->dterm_filter_type == FILTER_BIQUAD) {
for (int axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
}
BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2
const float dT = (float)targetPidLooptime * 0.000001f;
if (pidProfile->dterm_filter_type == FILTER_FIR) {
for (int axis = 0; axis < 3; axis++) firFilterDenoiseInit(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
if (pidProfile->dterm_notch_hz == 0) {
dtermNotchFilterApplyFn = nullFilterApply;
} else {
dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff);
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
dtermFilterNotch[axis] = &biquadFilterNotch[axis];
biquadFilterInit(dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH);
}
lowpassFilterType = pidProfile->dterm_filter_type;
}
if (pidProfile->dterm_lpf_hz == 0) {
dtermLpfApplyFn = nullFilterApply;
} else {
switch (pidProfile->dterm_filter_type) {
default:
dtermLpfApplyFn = nullFilterApply;
break;
case FILTER_PT1:
dtermLpfApplyFn = (filterApplyFnPtr)pt1FilterApply;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
dtermFilterLpf[axis] = &pt1Filter[axis];
pt1FilterInit(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, dT);
}
break;
case FILTER_BIQUAD:
dtermLpfApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
dtermFilterLpf[axis] = &biquadFilter[axis];
biquadFilterInitLPF(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
}
break;
case FILTER_FIR:
dtermLpfApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
dtermFilterLpf[axis] = &denoisingFilter[axis];
firFilterDenoiseInit(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
}
break;
}
}
if (pidProfile->yaw_lpf_hz == 0) {
ptermYawFilterApplyFn = nullFilterApply;
} else {
ptermYawFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
ptermYawFilter = &pt1FilterYaw;
pt1FilterInit(ptermYawFilter, pidProfile->yaw_lpf_hz, dT);
}
}
@ -156,7 +201,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
// ----------PID controller----------
const float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
for (int axis = 0; axis < 3; axis++) {
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
static uint8_t configP[3], configI[3], configD[3];
@ -177,9 +222,9 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
}
// Limit abrupt yaw inputs / stops
float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity;
const float maxVelocity = (axis == FD_YAW) ? yawMaxVelocity : rollPitchMaxVelocity;
if (maxVelocity) {
float currentVelocity = setpointRate[axis] - previousSetpoint[axis];
const float currentVelocity = setpointRate[axis] - previousSetpoint[axis];
if (ABS(currentVelocity) > maxVelocity) {
setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity;
}
@ -210,30 +255,28 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
// --------low-level gyro-based PID based on 2DOF PID controller. ----------
// ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ----------
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
// ----- calculate error / angle rates ----------
// -----calculate error rate
const float errorRate = setpointRate[axis] - PVRate; // r - y
// -----calculate P component and add Dynamic Part based on stick input
float PTerm = Kp[axis] * errorRate * tpaFactor;
// -----calculate I component.
// -----calculate I component
// Reduce strong Iterm accumulation during higher stick inputs
float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
float setpointRateScaler = constrainf(1.0f - (ABS(setpointRate[axis]) / accumulationThreshold), 0.0f, 1.0f);
const float accumulationThreshold = (axis == FD_YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
const float setpointRateScaler = constrainf(1.0f - (ABS(setpointRate[axis]) / accumulationThreshold), 0.0f, 1.0f);
const float itermScaler = setpointRateScaler * kiThrottleGain;
// Handle All windup Scenarios
float ITerm = errorGyroIf[axis];
ITerm += Ki[axis] * errorRate * dT * itermScaler;;
// limit maximum integrator value to prevent WindUp
float itermScaler = setpointRateScaler * kiThrottleGain;
ITerm = constrainf(ITerm, -250.0f, 250.0f);
errorGyroIf[axis] = ITerm;
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * dT * itermScaler, -250.0f, 250.0f);
// I coefficient (I8) moved before integration to make limiting independent from PID settings
const float ITerm = errorGyroIf[axis];
//-----calculate D-term (Yaw D not yet supported)
float DTerm;
if (axis != YAW) {
// -----calculate D component (Yaw D not yet supported)
float DTerm = 0.0;
if (axis != FD_YAW) {
static float previousSetpoint[3];
float dynC = c[axis];
if (pidProfile->setpointRelaxRatio < 100) {
@ -248,38 +291,23 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
}
previousSetpoint[axis] = setpointRate[axis];
const float rD = dynC * setpointRate[axis] - PVRate; // cr - y
float delta = rD - lastRateError[axis];
// Divide rate change by dT to get differential (ie dr/dt)
const float delta = (rD - lastRateError[axis]) / dT;
lastRateError[axis] = rD;
// Divide delta by targetLooptime to get differential (ie dr/dt)
delta *= (1.0f / dT);
if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * tpaFactor;
// Filter delta
if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta);
if (pidProfile->dterm_lpf_hz) {
if (pidProfile->dterm_filter_type == FILTER_BIQUAD)
delta = biquadFilterApply(&dtermFilterLpf[axis], delta);
else if (pidProfile->dterm_filter_type == FILTER_PT1)
delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, dT);
else
delta = firFilterDenoiseUpdate(&dtermDenoisingState[axis], delta);
}
DTerm = Kd[axis] * delta * tpaFactor;
DEBUG_SET(DEBUG_DTERM_FILTER, axis, DTerm);
// apply filters
DTerm = dtermNotchFilterApplyFn(dtermFilterNotch[axis], DTerm);
DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm);
// -----calculate total PID output
axisPIDf[axis] = PTerm + ITerm + DTerm;
} else {
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, dT);
axisPIDf[axis] = PTerm + ITerm;
DTerm = 0.0f; // needed for blackbox
PTerm = ptermYawFilterApplyFn(ptermYawFilter, PTerm);
}
// -----calculate total PID output
axisPIDf[axis] = PTerm + ITerm + DTerm;
// Disable PID control at zero throttle
if (!pidStabilisationEnabled) axisPIDf[axis] = 0;