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

Fixes from review.

Browse source
This commit is contained in:
mikeller 2018-06-09 13:11:37 +12:00
parent db3c0d1447
commit d43c178488
3 changed files with 20 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

12
src/main/common/maths.c
View file

@ -130,6 +130,18 @@ int32_t applyDeadband(int32_t value, int32_t deadband)
return value; return value;
} }
float fapplyDeadband(float value, float deadband)
{
if (fabsf(value) < deadband) {
value = 0;
} else if (value > 0) {
value -= deadband;
} else if (value < 0) {
value += deadband;
}
return value;
}
void devClear(stdev_t *dev) void devClear(stdev_t *dev)
{ {
dev->m_n = 0; dev->m_n = 0;

1
src/main/common/maths.h
View file

@ -91,6 +91,7 @@ typedef union {
int gcd(int num, int denom); int gcd(int num, int denom);
float powerf(float base, int exp); float powerf(float base, int exp);
int32_t applyDeadband(int32_t value, int32_t deadband); int32_t applyDeadband(int32_t value, int32_t deadband);
float fapplyDeadband(float value, float deadband);
void devClear(stdev_t *dev); void devClear(stdev_t *dev);
void devPush(stdev_t *dev, float x); void devPush(stdev_t *dev, float x);

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

@ -815,8 +815,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) { if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) {
const float gyroTarget = pt1FilterApply(&windupLpf[axis], currentPidSetpoint); const float gyroTarget = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
const float gyroHpf = fabsf(currentPidSetpoint - gyroTarget); const float gyroHpf = fabsf(currentPidSetpoint - gyroTarget);
if (itermRelaxType == ITERM_RELAX_SETPOINT) { if (itermRelaxType == ITERM_RELAX_SETPOINT && gyroHpf < 60) {
itermErrorRate = (1 - MIN(1, fabsf(gyroHpf) / 60.0f)) * (currentPidSetpoint - gyroRate); itermErrorRate = (1 - gyroHpf / 60.0f) * (currentPidSetpoint - gyroRate);
} }
const float tolerance = gyroHpf * 1.0f; const float tolerance = gyroHpf * 1.0f;
if (axis == FD_ROLL) { if (axis == FD_ROLL) {
@ -826,14 +826,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
DEBUG_SET(DEBUG_ITERM_RELAX, 3, axisError[axis] * 10); DEBUG_SET(DEBUG_ITERM_RELAX, 3, axisError[axis] * 10);
} }
if (itermRelaxType == ITERM_RELAX_GYRO ) { if (itermRelaxType == ITERM_RELAX_GYRO ) {
const float gmax = gyroTarget + tolerance; itermErrorRate = fapplyDeadband(gyroTarget - gyroRate, tolerance);
const float gmin = gyroTarget - tolerance;
if (gyroRate >= gmin && gyroRate <= gmax) {
itermErrorRate = 0;
} else {
itermErrorRate = (gyroRate > gmax ? gmax : gmin ) - gyroRate;
}
} }
#if defined(USE_ABSOLUTE_CONTROL) #if defined(USE_ABSOLUTE_CONTROL)
@ -847,8 +840,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
} else { } else {
acErrorRate = acErrorRate2; acErrorRate = acErrorRate2;
} }
if (fabsf(acErrorRate*dT) > fabsf(axisError[axis]) ) { if (fabsf(acErrorRate * dT) > fabsf(axisError[axis]) ) {
acErrorRate = -axisError[axis]/dT; acErrorRate = -axisError[axis] / dT;
} }
} else { } else {
acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate; acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate;
@ -857,9 +850,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
} else } else
#endif // USE_ITERM_RELAX #endif // USE_ITERM_RELAX
{ {
itermErrorRate = currentPidSetpoint - gyroRate; itermErrorRate = currentPidSetpoint - gyroRate;
#if defined(USE_ABSOLUTE_CONTROL) #if defined(USE_ABSOLUTE_CONTROL)
acErrorRate = itermErrorRate; acErrorRate = itermErrorRate;
#endif // USE_ABSOLUTE_CONTROL #endif // USE_ABSOLUTE_CONTROL
} }