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

Merge branch 'master' into development

Browse source
This commit is contained in:
borisbstyle 2016-09-11 01:16:53 +02:00
commit 05908aad4d
36 changed files with 957 additions and 183 deletions
Download patch file Download diff file Expand all files Collapse all files

66
src/main/flight/pid_betaflight.c
View file

@ -50,7 +50,7 @@
#include "flight/gtune.h"
extern float rcInput[3];
extern float setpointRate[3];
extern float setpointRate[3], ptermSetpointRate[3];
extern float errorGyroIf[3];
extern bool pidStabilisationEnabled;
@ -67,12 +67,13 @@ float getdT(void);
// 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 pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{
float errorRate = 0, rP = 0, rD = 0, PVRate = 0;
float ITerm,PTerm,DTerm;
static float lastRateError[2];
static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3];
static float Kp[3], Ki[3], Kd[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3];
float delta;
int axis;
float horizonLevelStrength = 1;
@ -124,7 +125,6 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
Kp[axis] = PTERM_SCALE * pidProfile->P8[axis];
Ki[axis] = ITERM_SCALE * pidProfile->I8[axis];
Kd[axis] = DTERM_SCALE * pidProfile->D8[axis];
b[axis] = pidProfile->ptermSetpointWeight / 100.0f;
c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * getdT();
rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT();
@ -156,11 +156,11 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
#endif
if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode - control is angle based, so control loop is needed
setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f;
ptermSetpointRate[axis] = setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f;
} else {
// HORIZON mode - direct sticks control is applied to rate PID
// mix up angle error to desired AngleRate to add a little auto-level feel
setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f);
ptermSetpointRate[axis] = setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f);
}
}
@ -171,42 +171,10 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
// 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
rP = b[axis] * setpointRate[axis] - PVRate; // br - y
// Slowly restore original setpoint with more stick input
float diffRate = errorRate - rP;
rP += diffRate * rcInput[axis];
// Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount
float dynReduction = tpaFactor;
if (pidProfile->toleranceBand) {
const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f;
static uint8_t zeroCrossCount[3];
static uint8_t currentErrorPolarity[3];
if (ABS(errorRate) < pidProfile->toleranceBand) {
if (zeroCrossCount[axis]) {
if (currentErrorPolarity[axis] == POSITIVE_ERROR) {
if (errorRate < 0 ) {
zeroCrossCount[axis]--;
currentErrorPolarity[axis] = NEGATIVE_ERROR;
}
} else {
if (errorRate > 0 ) {
zeroCrossCount[axis]--;
currentErrorPolarity[axis] = POSITIVE_ERROR;
}
}
} else {
dynReduction *= constrainf(ABS(errorRate) / pidProfile->toleranceBand, minReduction, 1.0f);
}
} else {
zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount;
currentErrorPolarity[axis] = (errorRate > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR;
}
}
rP = ptermSetpointRate[axis] - PVRate; // br - y
// -----calculate P component
PTerm = Kp[axis] * rP * dynReduction;
PTerm = Kp[axis] * rP * tpaFactor;
// -----calculate I component.
// Reduce strong Iterm accumulation during higher stick inputs
@ -223,13 +191,7 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
ITerm = errorGyroIf[axis];
//-----calculate D-term (Yaw D not yet supported)
if (axis == YAW) {
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
axisPID[axis] = lrintf(PTerm + ITerm);
DTerm = 0.0f; // needed for blackbox
} else {
if (axis != YAW) {
rD = c[axis] * setpointRate[axis] - PVRate; // cr - y
delta = rD - lastRateError[axis];
lastRateError[axis] = rD;
@ -237,7 +199,7 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
// Divide delta by targetLooptime to get differential (ie dr/dt)
delta *= (1.0f / getdT());
if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * dynReduction;
if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * tpaFactor;
// Filter delta
if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta);
@ -250,10 +212,16 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
}
}
DTerm = Kd[axis] * delta * dynReduction;
DTerm = Kd[axis] * delta * tpaFactor;
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900);
} else {
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
axisPID[axis] = lrintf(PTerm + ITerm);
DTerm = 0.0f; // needed for blackbox
}
// Disable PID control at zero throttle