Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 08:15:30 +03:00
Code Issues Wiki Activity

Remove double define // name consistency

Browse source
This commit is contained in:
borisbstyle 2016-11-30 21:26:58 +01:00
parent cbc8efd16a
commit 28ce3081c6
1 changed files with 4 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -166,7 +166,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, uint16_t midrc) const rollAndPitchTrims_t *angleTrim, uint16_t midrc)
{ {
static float lastRateError[2]; static float previousRateError[2];
static float previousSetpoint[3]; static float previousSetpoint[3];
float horizonLevelStrength = 1; float horizonLevelStrength = 1;
@ -212,7 +212,6 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
if (ABS(currentVelocity) > maxVelocity) { if (ABS(currentVelocity) > maxVelocity) {
setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity; setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity;
} }
previousSetpoint[axis] = setpointRate[axis];
} }
// Yaw control is GYRO based, direct sticks control is applied to rate PID // Yaw control is GYRO based, direct sticks control is applied to rate PID
@ -261,7 +260,6 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
// -----calculate D component (Yaw D not yet supported) // -----calculate D component (Yaw D not yet supported)
float DTerm = 0.0; float DTerm = 0.0;
if (axis != FD_YAW) { if (axis != FD_YAW) {
static float previousSetpoint[3];
float dynC = c[axis]; float dynC = c[axis];
if (pidProfile->setpointRelaxRatio < 100) { if (pidProfile->setpointRelaxRatio < 100) {
dynC = c[axis]; dynC = c[axis];
@ -273,11 +271,10 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
dynC = dynC * powerf(rcInput[axis], 2) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]); dynC = dynC * powerf(rcInput[axis], 2) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]);
} }
} }
previousSetpoint[axis] = setpointRate[axis];
const float rD = dynC * setpointRate[axis] - PVRate; // cr - y const float rD = dynC * setpointRate[axis] - PVRate; // cr - y
// Divide rate change by dT to get differential (ie dr/dt) // Divide rate change by dT to get differential (ie dr/dt)
const float delta = (rD - lastRateError[axis]) / dT; const float delta = (rD - previousRateError[axis]) / dT;
lastRateError[axis] = rD; previousRateError[axis] = rD;
DTerm = Kd[axis] * delta * tpaFactor; DTerm = Kd[axis] * delta * tpaFactor;
DEBUG_SET(DEBUG_DTERM_FILTER, axis, DTerm); DEBUG_SET(DEBUG_DTERM_FILTER, axis, DTerm);
@ -289,6 +286,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
} else { } else {
PTerm = ptermYawFilterApplyFn(ptermYawFilter, PTerm); PTerm = ptermYawFilterApplyFn(ptermYawFilter, PTerm);
} }
previousSetpoint[axis] = setpointRate[axis];
// -----calculate total PID output // -----calculate total PID output
axisPIDf[axis] = PTerm + ITerm + DTerm; axisPIDf[axis] = PTerm + ITerm + DTerm;