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

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;
}
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)
{
dev->m_n = 0;

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

@ -91,6 +91,7 @@ typedef union {
int gcd(int num, int denom);
float powerf(float base, int exp);
int32_t applyDeadband(int32_t value, int32_t deadband);
float fapplyDeadband(float value, float deadband);
void devClear(stdev_t *dev);
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 )) {
const float gyroTarget = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
const float gyroHpf = fabsf(currentPidSetpoint - gyroTarget);
if (itermRelaxType == ITERM_RELAX_SETPOINT) {
itermErrorRate = (1 - MIN(1, fabsf(gyroHpf) / 60.0f)) * (currentPidSetpoint - gyroRate);
if (itermRelaxType == ITERM_RELAX_SETPOINT && gyroHpf < 60) {
itermErrorRate = (1 - gyroHpf / 60.0f) * (currentPidSetpoint - gyroRate);
}
const float tolerance = gyroHpf * 1.0f;
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);
}
if (itermRelaxType == ITERM_RELAX_GYRO ) {
const float gmax = gyroTarget + tolerance;
const float gmin = gyroTarget - tolerance;
if (gyroRate >= gmin && gyroRate <= gmax) {
itermErrorRate = 0;
} else {
itermErrorRate = (gyroRate > gmax ? gmax : gmin ) - gyroRate;
}
itermErrorRate = fapplyDeadband(gyroTarget - gyroRate, tolerance);
}
#if defined(USE_ABSOLUTE_CONTROL)
@ -847,8 +840,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
} else {
acErrorRate = acErrorRate2;
}
if (fabsf(acErrorRate*dT) > fabsf(axisError[axis]) ) {
acErrorRate = -axisError[axis]/dT;
if (fabsf(acErrorRate * dT) > fabsf(axisError[axis]) ) {
acErrorRate = -axisError[axis] / dT;
}
} else {
acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate;
@ -857,9 +850,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
} else
#endif // USE_ITERM_RELAX
{
itermErrorRate = currentPidSetpoint - gyroRate;
itermErrorRate = currentPidSetpoint - gyroRate;
#if defined(USE_ABSOLUTE_CONTROL)
acErrorRate = itermErrorRate;
acErrorRate = itermErrorRate;
#endif // USE_ABSOLUTE_CONTROL
}