Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 05:45:31 +03:00
Code Issues Wiki Activity

Some fixes.

Browse source
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
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; float itermErrorRate = 0.0f;
#if defined(USE_ITERM_RELAX) #if defined(USE_ITERM_RELAX)
float stickSetpoint = currentPidSetpoint;
if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) { if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) {
const float gyroTarget = pt1FilterApply(&windupLpf[axis], stickSetpoint); const float gyroTarget = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
const float gyroHpf = fabsf(stickSetpoint - gyroTarget); const float gyroHpf = fabsf(currentPidSetpoint - gyroTarget);
if (itermRelaxType == ITERM_RELAX_SETPOINT) { 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; const float tolerance = gyroHpf * 1.0f;
if (axis == FD_ROLL) { if (axis == FD_ROLL) {
@ -831,9 +830,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
const float gmin = gyroTarget - tolerance; const float gmin = gyroTarget - tolerance;
if (gyroRate >= gmin && gyroRate <= gmax) { if (gyroRate >= gmin && gyroRate <= gmax) {
itermErrorRate = 0; itermErrorRate = 0;
} else { } 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; uint8_t D;
} pid8_t; } pid8_t;
typedef enum typedef enum {
{
ITERM_RELAX_OFF, ITERM_RELAX_OFF,
ITERM_RELAX_RP, ITERM_RELAX_RP,
ITERM_RELAX_RPY ITERM_RELAX_RPY
} itermRelax_e; } itermRelax_e;
typedef enum typedef enum {
{
ITERM_RELAX_GYRO, ITERM_RELAX_GYRO,
ITERM_RELAX_SETPOINT ITERM_RELAX_SETPOINT
} itermRelaxType_e; } itermRelaxType_e;