Smooth anti gravity

src/main/blackbox/blackbox.c

@@ -1283,6 +1283,7 @@ static bool blackboxWriteSysinfo(void)
         BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle", "%d",                currentPidProfile->pidAtMinThrottle);
 
         // Betaflight PID controller parameters
+        BLACKBOX_PRINT_HEADER_LINE("anti_gravity_mode", "%d",               currentPidProfile->antiGravityMode);
         BLACKBOX_PRINT_HEADER_LINE("anti_gravity_threshold", "%d",          currentPidProfile->itermThrottleThreshold);
         BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain", "%d",               currentPidProfile->itermAcceleratorGain);
         BLACKBOX_PRINT_HEADER_LINE("setpoint_relaxation_ratio", "%d",       currentPidProfile->setpointRelaxRatio);

src/main/build/debug.c

@@ -74,4 +74,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "RC_SMOOTHING",
     "RX_SIGNAL_LOSS",
     "RC_SMOOTHING_RATE",
+    "ANTI_GRAVITY",
 };

src/main/build/debug.h

@@ -92,6 +92,7 @@ typedef enum {
     DEBUG_RC_SMOOTHING,
     DEBUG_RX_SIGNAL_LOSS,
     DEBUG_RC_SMOOTHING_RATE,
+    DEBUG_ANTI_GRAVITY,
     DEBUG_COUNT
 } debugType_e;
 

src/main/fc/fc_core.c

@@ -837,6 +837,9 @@ bool processRx(timeUs_t currentTimeUs)
         beeper(BEEPER_RC_SMOOTHING_INIT_FAIL);
     }
 #endif
+
+    pidSetAntiGravityState(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY));
+    
     return true;
 }
 

src/main/fc/fc_rc.c

@@ -212,11 +212,13 @@ static void checkForThrottleErrorResetState(uint16_t rxRefreshRate)
 
     const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index];
 
+    if (currentPidProfile->antiGravityMode == ANTI_GRAVITY_STEP) {
         if (ABS(rcCommandSpeed) > throttleVelocityThreshold) {
             pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain));
         } else {
             pidSetItermAccelerator(1.0f);
         }
+    }
 }
 
 FAST_CODE uint8_t processRcInterpolation(void)
@@ -536,7 +538,7 @@ FAST_CODE void processRcCommand(void)
 {
     uint8_t updatedChannel;
 
-    if (isRXDataNew && isAntiGravityModeActive()) {
+    if (isRXDataNew && pidAntiGravityEnabled()) {
         checkForThrottleErrorResetState(currentRxRefreshRate);
     }
 

src/main/fc/rc_modes.c

@@ -62,10 +62,6 @@ bool isAirmodeActive(void) {
     return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE));
 }
 
-bool isAntiGravityModeActive(void) {
-    return (IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY));
-}
-
 bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) {
     if (!IS_RANGE_USABLE(range)) {
         return false;

src/main/fc/rc_modes.h

@@ -132,7 +132,6 @@ bool IS_RC_MODE_ACTIVE(boxId_e boxId);
 void rcModeUpdate(boxBitmask_t *newState);
 
 bool isAirmodeActive(void);
-bool isAntiGravityModeActive(void);
 
 bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
 void updateActivatedModes(void);

src/main/flight/mixer.c

@@ -806,10 +806,12 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
         motorMix[i] = mix;
     }
 
+    pidUpdateAntiGravityThrottleFilter(throttle);
+    
 #if defined(USE_THROTTLE_BOOST)
     if (throttleBoost > 0.0f) {
-        float throttlehpf = throttle - pt1FilterApply(&throttleLpf, throttle);
+        const float throttleHpf = throttle - pt1FilterApply(&throttleLpf, throttle);
-        throttle = constrainf(throttle + throttleBoost * throttlehpf, 0.0f, 1.0f);
+        throttle = constrainf(throttle + throttleBoost * throttleHpf, 0.0f, 1.0f);
     }
 #endif
 

src/main/flight/mixer.h

@@ -136,3 +136,4 @@ uint16_t convertMotorToExternal(float motorValue);
 bool mixerIsTricopter(void);
 
 void mixerSetThrottleAngleCorrection(int correctionValue);
+

src/main/flight/pid.c

@@ -68,6 +68,12 @@ static FAST_RAM_ZERO_INIT bool inCrashRecoveryMode = false;
 static FAST_RAM_ZERO_INIT float dT;
 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_ZERO_INIT bool antiGravityEnabled;
+
 PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2);
 
 #ifdef STM32F10X
@@ -96,6 +102,8 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
 #define ACRO_TRAINER_SETPOINT_LIMIT       1000.0f // Limit the correcting setpoint
 #endif // USE_ACRO_TRAINER
 
+#define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15  // The anti gravity throttle highpass filter cutoff
+
 PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 4);
 
 void resetPidProfile(pidProfile_t *pidProfile)
@@ -130,7 +138,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .dtermSetpointWeight = 60,
         .yawRateAccelLimit = 100,
         .rateAccelLimit = 0,
-        .itermThrottleThreshold = 350,
+        .itermThrottleThreshold = 250,
         .itermAcceleratorGain = 5000,
         .crash_time = 500,          // ms
         .crash_delay = 0,           // ms
@@ -158,6 +166,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .abs_control_gain = 0,
         .abs_control_limit = 90,
         .abs_control_error_limit = 20,
+        .antiGravityMode = ANTI_GRAVITY_SMOOTH,
     );
 }
 
@@ -182,9 +191,9 @@ void pidSetItermAccelerator(float newItermAccelerator)
     itermAccelerator = newItermAccelerator;
 }
 
-float pidItermAccelerator(void)
+bool pidOsdAntiGravityActive(void)
 {
-    return itermAccelerator;
+    return (itermAccelerator > antiGravityOSDCutoff);
 }
 
 void pidStabilisationState(pidStabilisationState_e pidControllerState)
@@ -222,6 +231,8 @@ static FAST_RAM_ZERO_INIT uint8_t rcSmoothingDebugAxis;
 static FAST_RAM_ZERO_INIT uint8_t rcSmoothingFilterType;
 #endif // USE_RC_SMOOTHING_FILTER
 
+static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf;
+
 void pidInitFilters(const pidProfile_t *pidProfile)
 {
     BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2
@@ -306,6 +317,8 @@ void pidInitFilters(const pidProfile_t *pidProfile)
         }
     }
 #endif
+
+    pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT));
 }
 
 #ifdef USE_RC_SMOOTHING_FILTER
@@ -402,6 +415,13 @@ static FAST_RAM_ZERO_INIT int acroTrainerAxisState[2];  // only need roll and pi
 static FAST_RAM_ZERO_INIT float acroTrainerGain;
 #endif // USE_ACRO_TRAINER
 
+void pidUpdateAntiGravityThrottleFilter(float throttle)
+{
+    if (antiGravityMode) {
+        antiGravityThrottleHpf = throttle - pt1FilterApply(&antiGravityThrottleLpf, throttle);
+    }
+}
+
 void pidInitConfig(const pidProfile_t *pidProfile)
 {
     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@@ -426,6 +446,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
     maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT;
     const float ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
     ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint);
+    itermAcceleratorGain = pidProfile->itermAcceleratorGain;
     crashTimeLimitUs = pidProfile->crash_time * 1000;
     crashTimeDelayUs = pidProfile->crash_delay * 1000;
     crashRecoveryAngleDeciDegrees = pidProfile->crash_recovery_angle * 10;
@@ -439,6 +460,18 @@ void pidInitConfig(const pidProfile_t *pidProfile)
     throttleBoost = pidProfile->throttle_boost * 0.1f;
 #endif
     itermRotation = pidProfile->iterm_rotation;
+    antiGravityMode = pidProfile->antiGravityMode;
+    
+    // Calculate the anti-gravity value that will trigger the OSD display.
+    // For classic AG it's either 1.0 for off and > 1.0 for on.
+    // 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;
+    if (antiGravityMode == ANTI_GRAVITY_SMOOTH) {
+        antiGravityOSDCutoff += ((itermAcceleratorGain - 1000) / 1000.0f) * 0.25f;
+    }
+
 #if defined(USE_SMART_FEEDFORWARD)
     smartFeedforward = pidProfile->smart_feedforward;
 #endif
@@ -780,6 +813,13 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
 
     // 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));
+
     const float dynCi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f) * dT * itermAccelerator;
 
     // Dynamic d component, enable 2-DOF PID controller only for rate mode
@@ -1028,3 +1068,17 @@ void pidSetAcroTrainerState(bool newState)
     }
 }
 #endif // USE_ACRO_TRAINER
+
+void pidSetAntiGravityState(bool newState)
+{
+    if (newState != antiGravityEnabled) {
+        // reset the accelerator on state changes
+        itermAccelerator = 1.0f;
+    }
+    antiGravityEnabled = newState;
+}
+
+bool pidAntiGravityEnabled(void)
+{
+    return antiGravityEnabled;
+}

src/main/flight/pid.h

@@ -76,6 +76,12 @@ typedef struct pid8_s {
     uint8_t D;
 } pid8_t;
 
+typedef enum {
+    ANTI_GRAVITY_OFF = 0,
+    ANTI_GRAVITY_STEP,
+    ANTI_GRAVITY_SMOOTH
+} antiGravityMode_e;
+
 typedef enum {
     ITERM_RELAX_OFF,
     ITERM_RELAX_RP,
@@ -108,6 +114,7 @@ typedef struct pidProfile_s {
     uint8_t horizon_tilt_expert_mode;       // OFF or ON
 
     // Betaflight PID controller parameters
+    uint8_t  antiGravityMode;             // type of anti gravity method
     uint16_t itermThrottleThreshold;        // max allowed throttle delta before iterm accelerated in ms
     uint16_t itermAcceleratorGain;          // Iterm Accelerator Gain when itermThrottlethreshold is hit
     uint16_t dtermSetpointWeight;           // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > aggressive derivative)
@@ -176,7 +183,6 @@ extern pt1Filter_t throttleLpf;
 void pidResetITerm(void);
 void pidStabilisationState(pidStabilisationState_e pidControllerState);
 void pidSetItermAccelerator(float newItermAccelerator);
-float pidItermAccelerator(void);
 void pidInitFilters(const pidProfile_t *pidProfile);
 void pidInitConfig(const pidProfile_t *pidProfile);
 void pidInit(const pidProfile_t *pidProfile);
@@ -186,3 +192,8 @@ void pidAcroTrainerInit(void);
 void pidSetAcroTrainerState(bool newState);
 void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType);
 void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff);
+void pidUpdateAntiGravityThrottleFilter(float throttle);
+bool pidOsdAntiGravityActive(void);
+bool pidOsdAntiGravityMode(void);
+void pidSetAntiGravityState(bool newState);
+bool pidAntiGravityEnabled(void);

src/main/interface/settings.c

@@ -273,6 +273,11 @@ static const char * const lookupTableDtermLowpassType[] = {
     "BIQUAD",
 };
 
+static const char * const lookupTableAntiGravityMode[] = {
+    "OFF",
+    "STEP",
+    "SMOOTH",
+};
 
 static const char * const lookupTableFailsafe[] = {
     "AUTO-LAND", "DROP", "GPS-RESCUE"
@@ -413,6 +418,7 @@ const lookupTableEntry_t lookupTables[] = {
     LOOKUP_TABLE_ENTRY(lookupTableRcInterpolationChannels),
     LOOKUP_TABLE_ENTRY(lookupTableLowpassType),
     LOOKUP_TABLE_ENTRY(lookupTableDtermLowpassType),
+    LOOKUP_TABLE_ENTRY(lookupTableAntiGravityMode),
     LOOKUP_TABLE_ENTRY(lookupTableFailsafe),
     LOOKUP_TABLE_ENTRY(lookupTableFailsafeSwitchMode),
     LOOKUP_TABLE_ENTRY(lookupTableCrashRecovery),
@@ -784,6 +790,7 @@ const clivalue_t valueTable[] = {
     { "dterm_notch_cutoff",         VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_cutoff) },
     { "vbat_pid_gain",              VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, vbatPidCompensation) },
     { "pid_at_min_throttle",        VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, pidAtMinThrottle) },
+    { "anti_gravity_mode",          VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ANTI_GRAVITY_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, antiGravityMode) },
     { "anti_gravity_threshold",     VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 20, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermThrottleThreshold) },
     { "anti_gravity_gain",          VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1000, 30000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermAcceleratorGain) },
     { "setpoint_relax_ratio",       VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, setpointRelaxRatio) },

src/main/interface/settings.h

@@ -68,6 +68,7 @@ typedef enum {
     TABLE_RC_INTERPOLATION_CHANNELS,
     TABLE_LOWPASS_TYPE,
     TABLE_DTERM_LOWPASS_TYPE,
+    TABLE_ANTI_GRAVITY_MODE,
     TABLE_FAILSAFE,
     TABLE_FAILSAFE_SWITCH_MODE,
     TABLE_CRASH_RECOVERY,

src/main/io/osd.c

@@ -599,7 +599,7 @@ static bool osdDrawSingleElement(uint8_t item)
 
     case OSD_ANTI_GRAVITY:
         {
-            if (pidItermAccelerator() > 1.0f) {
+            if (pidOsdAntiGravityActive()) {
                 strcpy(buff, "AG");
             }
 

src/test/unit/arming_prevention_unittest.cc

@@ -850,4 +850,5 @@ extern "C" {
     void rescheduleTask(cfTaskId_e, uint32_t) {}
     bool usbCableIsInserted(void) { return false; }
     bool usbVcpIsConnected(void) { return false; }
+    void pidSetAntiGravityState(bool) {}
 }

src/test/unit/osd_unittest.cc

@@ -1036,5 +1036,5 @@ extern "C" {
         return false;
     }
 
-    float pidItermAccelerator(void) { return 1.0; }
+    bool pidOsdAntiGravityActive(void) { return false; }
 }

src/test/unit/pid_unittest.cc

@@ -105,7 +105,8 @@ void setDefaultTestSettings(void) {
     pidProfile->dtermSetpointWeight = 0;
     pidProfile->yawRateAccelLimit = 100;
     pidProfile->rateAccelLimit = 0;
-    pidProfile->itermThrottleThreshold = 350;
+    pidProfile->antiGravityMode = ANTI_GRAVITY_SMOOTH;
+    pidProfile->itermThrottleThreshold = 250;
     pidProfile->itermAcceleratorGain = 1000;
     pidProfile->crash_time = 500;
     pidProfile->crash_delay = 0;