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

Some fixes.

This commit is contained in:
Michael Keller 2018-06-08 21:18:54 +12:00
parent fefedbd686
commit db3c0d1447
2 changed files with 7 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -812,12 +812,11 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
float itermErrorRate = 0.0f;
#if defined(USE_ITERM_RELAX)
float stickSetpoint = currentPidSetpoint;
if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) {
const float gyroTarget = pt1FilterApply(&windupLpf[axis], stickSetpoint);
const float gyroHpf = fabsf(stickSetpoint - gyroTarget);
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)) * (stickSetpoint - gyroRate);
itermErrorRate = (1 - MIN(1, fabsf(gyroHpf) / 60.0f)) * (currentPidSetpoint - gyroRate);
}
const float tolerance = gyroHpf * 1.0f;
if (axis == FD_ROLL) {
@ -831,9 +830,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
const float gmin = gyroTarget - tolerance;
if (gyroRate >= gmin && gyroRate <= gmax) {
itermErrorRate = 0;
itermErrorRate = 0;
} else {
itermErrorRate = (gyroRate > gmax ? gmax : gmin ) - gyroRate;
itermErrorRate = (gyroRate > gmax ? gmax : gmin ) - gyroRate;
}
}

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

@ -76,15 +76,13 @@ typedef struct pid8_s {
uint8_t D;
} pid8_t;
typedef enum
{
typedef enum {
ITERM_RELAX_OFF,
ITERM_RELAX_RP,
ITERM_RELAX_RPY
} itermRelax_e;
typedef enum
{
typedef enum {
ITERM_RELAX_GYRO,
ITERM_RELAX_SETPOINT
} itermRelaxType_e;