From 94c7109a4c02e152d0e1d5a06a57ed0146e13d76 Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Mon, 9 Jul 2018 21:03:16 +1000 Subject: [PATCH] Changes, thanks mikeller and jirif --- src/main/flight/pid.c | 12 +++++------- src/main/flight/pid.h | 5 ++--- src/main/interface/settings.c | 3 +-- 3 files changed, 8 insertions(+), 12 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 2a12cac32..9a4eb3e82 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -71,7 +71,7 @@ static FAST_RAM_ZERO_INIT float pidFrequency; static FAST_RAM_ZERO_INIT uint8_t antiGravityMode; static FAST_RAM_ZERO_INIT float antiGravityThrottleHpf; static FAST_RAM_ZERO_INIT uint16_t itermAcceleratorGain; -static FAST_RAM float antiGravityOSDCutoff = 1.0f; +static FAST_RAM float antiGravityOsdCutoff = 1.0f; static FAST_RAM_ZERO_INIT bool antiGravityEnabled; PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2); @@ -193,7 +193,7 @@ void pidSetItermAccelerator(float newItermAccelerator) bool pidOsdAntiGravityActive(void) { - return (itermAccelerator > antiGravityOSDCutoff); + return (itermAccelerator > antiGravityOsdCutoff); } void pidStabilisationState(pidStabilisationState_e pidControllerState) @@ -467,9 +467,9 @@ void pidInitConfig(const pidProfile_t *pidProfile) // For the new AG it's a continuous floating value so we want to trigger the OSD // display when it exceeds 25% of its possible range. This gives a useful indication // of AG activity without excessive display. - antiGravityOSDCutoff = 1.0f; + antiGravityOsdCutoff = 1.0f; if (antiGravityMode == ANTI_GRAVITY_SMOOTH) { - antiGravityOSDCutoff += ((itermAcceleratorGain - 1000) / 1000.0f) * 0.25f; + antiGravityOsdCutoff += ((itermAcceleratorGain - 1000) / 1000.0f) * 0.25f; } #if defined(USE_SMART_FEEDFORWARD) @@ -812,14 +812,12 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT #endif // Dynamic i component, - // gradually scale back integration when above windup point if ((antiGravityMode == ANTI_GRAVITY_SMOOTH) && antiGravityEnabled) { itermAccelerator = 1 + fabsf(antiGravityThrottleHpf) * 0.01f * (itermAcceleratorGain - 1000); DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(antiGravityThrottleHpf * 1000)); } - DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(itermAccelerator * 1000)); - + // gradually scale back integration when above windup point const float dynCi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f) * dT * itermAccelerator; // Dynamic d component, enable 2-DOF PID controller only for rate mode diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 3442e9d7f..25a17ec20 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -77,9 +77,8 @@ typedef struct pid8_s { } pid8_t; typedef enum { - ANTI_GRAVITY_OFF = 0, - ANTI_GRAVITY_STEP, - ANTI_GRAVITY_SMOOTH + ANTI_GRAVITY_SMOOTH, + ANTI_GRAVITY_STEP } antiGravityMode_e; typedef enum { diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 939fb33d3..0b5fcb1c1 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -274,9 +274,8 @@ static const char * const lookupTableDtermLowpassType[] = { }; static const char * const lookupTableAntiGravityMode[] = { - "OFF", - "STEP", "SMOOTH", + "STEP", }; static const char * const lookupTableFailsafe[] = {