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

Add Iterm Relax INC modes

Browse source
This commit is contained in:
Miguel Angel Mulero Martinez 2018-06-15 15:08:09 +02:00
parent 9a06900c83
commit d63eecffaa
3 changed files with 17 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -809,19 +809,27 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
float acCorrection = 0; float acCorrection = 0;
float acErrorRate; float acErrorRate;
#endif #endif
float itermErrorRate = 0.0f;
const float ITerm = pidData[axis].I;
float itermErrorRate = currentPidSetpoint - gyroRate;
#if defined(USE_ITERM_RELAX) #if defined(USE_ITERM_RELAX)
if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) { if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY || itermRelax == ITERM_RELAX_RPY_INC)) {
const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint); const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf); const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
const float itermRelaxFactor = 1 - setpointHpf / 30.0f; const float itermRelaxFactor = 1 - setpointHpf / 30.0f;
if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < 30) {
itermErrorRate = itermRelaxFactor * (currentPidSetpoint - gyroRate); const bool isDecreasingI = ((ITerm > 0) && (itermErrorRate < 0)) || ((ITerm < 0) && (itermErrorRate > 0));
if ((itermRelax >= ITERM_RELAX_RP_INC) && isDecreasingI) {
// Do Nothing, use the precalculed itermErrorRate
} else if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < 30) {
itermErrorRate *= itermRelaxFactor;
} else if (itermRelaxType == ITERM_RELAX_GYRO ) { } else if (itermRelaxType == ITERM_RELAX_GYRO ) {
itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf); itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf);
} else {
itermErrorRate = 0.0f;
} }
if (axis == FD_ROLL) { if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointHpf)); DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointHpf));
DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(itermRelaxFactor * 100.0f)); DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(itermRelaxFactor * 100.0f));
@ -849,7 +857,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
} else } else
#endif // USE_ITERM_RELAX #endif // USE_ITERM_RELAX
{ {
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
@ -883,8 +890,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
} }
// -----calculate I component // -----calculate I component
const float ITerm = pidData[axis].I;
const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit); const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);
const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate); const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate);
if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) { if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) {

4
src/main/flight/pid.h
View file

@ -79,7 +79,9 @@ typedef struct pid8_s {
typedef enum { typedef enum {
ITERM_RELAX_OFF, ITERM_RELAX_OFF,
ITERM_RELAX_RP, ITERM_RELAX_RP,
ITERM_RELAX_RPY ITERM_RELAX_RPY,
ITERM_RELAX_RP_INC,
ITERM_RELAX_RPY_INC
} itermRelax_e; } itermRelax_e;
typedef enum { typedef enum {

2
src/main/interface/settings.c
View file

@ -339,7 +339,7 @@ static const char * const lookupTableVideoSystem[] = {
#if defined(USE_ITERM_RELAX) #if defined(USE_ITERM_RELAX)
static const char * const lookupTableItermRelax[] = { static const char * const lookupTableItermRelax[] = {
"OFF", "RP", "RPY" "OFF", "RP", "RPY", "RP_INC", "RPY_INC"
}; };
static const char * const lookupTableItermRelaxType[] = { static const char * const lookupTableItermRelaxType[] = {
"GYRO", "SETPOINT" "GYRO", "SETPOINT"