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

Merge pull request #1621 from martinbudden/bf_pid_tidy 

Tidy PID controller
This commit is contained in:
borisbstyle 2016-11-23 13:22:36 +01:00 committed by GitHub
parent 12e7d3ad8d ac6b83ad7d
commit cd5a8861f3
3 changed files with 36 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/fc/mw.c
View File

@ -101,8 +101,6 @@ static bool armingCalibrationWasInitialised;
float setpointRate[3];
float rcInput[3];
extern pidControllerFuncPtr pid_controller;
void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
{
masterConfig.accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
@ -695,7 +693,7 @@ void subTaskPidController(void)
&currentProfile->pidProfile,
masterConfig.max_angle_inclination,
&masterConfig.accelerometerTrims,
&masterConfig.rxConfig
masterConfig.rxConfig.midrc
);
if (debugMode == DEBUG_PIDLOOP) {debug[2] = micros() - startTime;}
}

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

@ -21,7 +21,6 @@
#include <platform.h>
#include "build/build_config.h"
#include "build/debug.h"
#include "common/axis.h"
@ -36,10 +35,6 @@
#include "flight/navigation.h"
#include "flight/gtune.h"
#include "io/gps.h"
#include "rx/rx.h"
#include "sensors/gyro.h"
#include "sensors/acceleration.h"
@ -47,8 +42,7 @@ extern float rcInput[3];
extern float setpointRate[3];
uint32_t targetPidLooptime;
bool pidStabilisationEnabled;
uint8_t PIDweight[3];
static bool pidStabilisationEnabled;
float axisPIDf[3];
@ -59,10 +53,7 @@ uint8_t PIDweight[3];
int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
#endif
int32_t errorGyroI[3];
float errorGyroIf[3];
pidControllerFuncPtr pid_controller; // which pid controller are we using
static float errorGyroIf[3];
void setTargetPidLooptime(uint32_t pidLooptime)
{
@ -72,7 +63,6 @@ void setTargetPidLooptime(uint32_t pidLooptime)
void pidResetErrorGyroState(void)
{
for (int axis = 0; axis < 3; axis++) {
errorGyroI[axis] = 0;
errorGyroIf[axis] = 0.0f;
}
}
@ -82,14 +72,6 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState)
pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false;
}
float getdT(void)
{
static float dT;
if (!dT) dT = (float)targetPidLooptime * 0.000001f;
return dT;
}
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
pt1Filter_t deltaFilter[3];
@ -100,23 +82,23 @@ bool dtermNotchInitialised;
bool dtermBiquadLpfInitialised;
firFilterDenoise_t dtermDenoisingState[3];
static void pidInitFilters(const pidProfile_t *pidProfile) {
int axis;
static void pidInitFilters(const pidProfile_t *pidProfile)
{
static uint8_t lowpassFilterType;
if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) {
float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff);
for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH);
for (int axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH);
dtermNotchInitialised = true;
}
if ((pidProfile->dterm_filter_type != lowpassFilterType) && pidProfile->dterm_lpf_hz) {
if (pidProfile->dterm_filter_type == FILTER_BIQUAD) {
for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
for (int axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
}
if (pidProfile->dterm_filter_type == FILTER_FIR) {
for (axis = 0; axis < 3; axis++) firFilterDenoiseInit(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
for (int axis = 0; axis < 3; axis++) firFilterDenoiseInit(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
}
lowpassFilterType = pidProfile->dterm_filter_type;
}
@ -125,24 +107,25 @@ static void pidInitFilters(const pidProfile_t *pidProfile) {
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
// Based on 2DOF reference design (matlab)
void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
const rollAndPitchTrims_t *angleTrim, uint16_t midrc)
{
float errorRate = 0, rD = 0, PVRate = 0, dynC;
float ITerm,PTerm,DTerm;
static float lastRateError[2];
static float Kp[3], Ki[3], Kd[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3], relaxFactor[3];
float delta;
int axis;
float horizonLevelStrength = 1;
static float Kp[3], Ki[3], Kd[3], c[3];
static float rollPitchMaxVelocity, yawMaxVelocity;
static float previousSetpoint[3], relaxFactor[3];
static float dT;
float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
if (!dT) {
dT = (float)targetPidLooptime * 0.000001f;
}
pidInitFilters(pidProfile);
float horizonLevelStrength = 1;
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions
const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc));
const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, midrc));
const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, midrc));
const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle);
// Progressively turn off the horizon self level strength as the stick is banged over
horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection
@ -172,7 +155,8 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
}
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
const float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
for (int axis = 0; axis < 3; axis++) {
static uint8_t configP[3], configI[3], configD[3];
@ -184,8 +168,8 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
Kd[axis] = DTERM_SCALE * pidProfile->D8[axis];
c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
relaxFactor[axis] = 1.0f - (pidProfile->setpointRelaxRatio / 100.0f);
yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * getdT();
rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT();
yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * dT;
rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * dT;
configP[axis] = pidProfile->P8[axis];
configI[axis] = pidProfile->I8[axis];
@ -222,16 +206,16 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
}
}
PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec
const float PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec
// --------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 ----------
errorRate = setpointRate[axis] - PVRate; // r - y
const float errorRate = setpointRate[axis] - PVRate; // r - y
// -----calculate P component and add Dynamic Part based on stick input
PTerm = Kp[axis] * errorRate * tpaFactor;
float PTerm = Kp[axis] * errorRate * tpaFactor;
// -----calculate I component.
// Reduce strong Iterm accumulation during higher stick inputs
@ -242,15 +226,16 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
// limit maximum integrator value to prevent WindUp
float itermScaler = setpointRateScaler * kiThrottleGain;
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f);
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
ITerm = errorGyroIf[axis];
const float ITerm = errorGyroIf[axis];
//-----calculate D-term (Yaw D not yet supported)
float DTerm;
if (axis != YAW) {
static float previousSetpoint[3];
dynC = c[axis];
float dynC = c[axis];
if (pidProfile->setpointRelaxRatio < 100) {
dynC = c[axis];
if (setpointRate[axis] > 0) {
@ -262,12 +247,12 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
}
}
previousSetpoint[axis] = setpointRate[axis];
rD = dynC * setpointRate[axis] - PVRate; // cr - y
delta = rD - lastRateError[axis];
const float rD = dynC * setpointRate[axis] - PVRate; // cr - y
float delta = rD - lastRateError[axis];
lastRateError[axis] = rD;
// Divide delta by targetLooptime to get differential (ie dr/dt)
delta *= (1.0f / getdT());
delta *= (1.0f / dT);
if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * tpaFactor;
@ -278,7 +263,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
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, getdT());
delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, dT);
else
delta = firFilterDenoiseUpdate(&dtermDenoisingState[axis], delta);
}
@ -288,7 +273,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
// -----calculate total PID output
axisPIDf[axis] = PTerm + ITerm + DTerm;
} else {
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, dT);
axisPIDf[axis] = PTerm + ITerm;

7
src/main/flight/pid.h
View File

@ -90,14 +90,9 @@ typedef struct pidProfile_s {
#endif
} pidProfile_t;
struct controlRateConfig_s;
union rollAndPitchTrims_u;
struct rxConfig_s;
typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); // pid controller function prototype
void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig);
const union rollAndPitchTrims_u *angleTrim, uint16_t midrc);
extern float axisPIDf[3];
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];