Add anti gravity as feature and mode

3.1.7 mergebacks

src/main/fc/cli.c

@@ -147,7 +147,7 @@ static const char * const featureNames[] = {
     "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
     "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD",
     "UNUSED", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE",
-    "SDCARD", "VTX", "RX_SPI", "SOFTSPI", "ESC_SENSOR", NULL
+    "SDCARD", "VTX", "RX_SPI", "SOFTSPI", "ESC_SENSOR", "ANTI_GRAVITY", NULL
 };
 
 // sync this with rxFailsafeChannelMode_e

src/main/fc/config.h

@@ -61,6 +61,7 @@ typedef enum {
     FEATURE_RX_SPI = 1 << 25,
     FEATURE_SOFTSPI = 1 << 26,
     FEATURE_ESC_SENSOR = 1 << 27,
+    FEATURE_ANTI_GRAVITY = 1 << 28,
 } features_e;
 
 typedef struct systemConfig_s {

src/main/fc/fc_msp.c

@@ -126,7 +126,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXANGLE, "ANGLE;", 1 },
     { BOXHORIZON, "HORIZON;", 2 },
     { BOXBARO, "BARO;", 3 },
-    //{ BOXVARIO, "VARIO;", 4 },
+    { BOXANTIGRAVITY, "ANTI GRAVITY;", 4 },
     { BOXMAG, "MAG;", 5 },
     { BOXHEADFREE, "HEADFREE;", 6 },
     { BOXHEADADJ, "HEADADJ;", 7 },
@@ -322,6 +322,10 @@ void initActiveBoxIds(void)
         activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
     }
 
+    if (!feature(FEATURE_ANTI_GRAVITY)) {
+        activeBoxIds[activeBoxIdCount++] = BOXANTIGRAVITY;
+    }
+
     if (sensors(SENSOR_ACC)) {
         activeBoxIds[activeBoxIdCount++] = BOXANGLE;
         activeBoxIds[activeBoxIdCount++] = BOXHORIZON;
@@ -441,6 +445,7 @@ static uint32_t packFlightModeFlags(void)
         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE)) << BOXBLACKBOXERASE |
         IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE |
         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE |
+        IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY)) << BOXANTIGRAVITY |
         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX)) << BOXFPVANGLEMIX;
 
     uint32_t ret = 0;

src/main/fc/fc_rc.c

@@ -185,8 +185,9 @@ void processRcCommand(void)
 
     if (isRXDataNew) {
         currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000);
-        if (currentPidProfile->itermAcceleratorGain > 1.0f)
+        if (isAntiGravityModeActive()) {
             checkForThrottleErrorResetState(currentRxRefreshRate);
+        }
     }
 
     if (rxConfig()->rcInterpolation || flightModeFlags) {

src/main/fc/rc_controls.c

@@ -94,6 +94,10 @@ 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 isUsingSticksForArming(void)
 {
     return isUsingSticksToArm;

src/main/fc/rc_controls.h

@@ -26,7 +26,7 @@ typedef enum {
     BOXANGLE,
     BOXHORIZON,
     BOXBARO,
-    // BOXVARIO,
+    BOXANTIGRAVITY,
     BOXMAG,
     BOXHEADFREE,
     BOXHEADADJ,
@@ -190,6 +190,7 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
 void updateActivatedModes(void);
 
 bool isAirmodeActive(void);
+bool isAntiGravityModeActive(void);
 
 bool isUsingSticksForArming(void);
 

src/main/flight/mixer.c

@@ -539,13 +539,19 @@ void mixTable(pidProfile_t *pidProfile)
     const float motorOutputRange = motorOutputMax - motorOutputMin;
 
     float scaledAxisPIDf[3];
-    // Limit the PIDsum
+    // Calculate and Limit the PIDsum
-    scaledAxisPIDf[FD_ROLL] = constrainf(axisPIDf[FD_ROLL] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit);
+    scaledAxisPIDf[FD_ROLL] =
-    scaledAxisPIDf[FD_PITCH] = constrainf(axisPIDf[FD_PITCH] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit);
+        constrainf((axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL]) / PID_MIXER_SCALING,
-    scaledAxisPIDf[FD_YAW] = constrainf(axisPIDf[FD_YAW] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimitYaw);
+        -pidProfile->pidSumLimit, pidProfile->pidSumLimit);
+    scaledAxisPIDf[FD_PITCH] =
+        constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING,
+        -pidProfile->pidSumLimit, pidProfile->pidSumLimit);
+    scaledAxisPIDf[FD_YAW] =
+        constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) / PID_MIXER_SCALING,
+        -pidProfile->pidSumLimit, pidProfile->pidSumLimitYaw);
 
     // Calculate voltage compensation
-    const float vbatCompensationFactor = pidProfile->vbatPidCompensation  ? calculateVbatPidCompensation() : 1.0f;
+    const float vbatCompensationFactor = (pidProfile->vbatPidCompensation)  ? calculateVbatPidCompensation() : 1.0f;
 
     // Find roll/pitch/yaw desired output
     float motorMix[MAX_SUPPORTED_MOTORS];

src/main/flight/pid.c

@@ -51,13 +51,7 @@
 uint32_t targetPidLooptime;
 static bool pidStabilisationEnabled;
 
-float axisPIDf[3];
+float axisPID_P[3], axisPID_I[3], axisPID_D[3];
-
-#ifdef BLACKBOX
-int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
-#endif
-
-static float previousGyroIf[3];
 
 static float dT;
 
@@ -145,7 +139,7 @@ void pidSetTargetLooptime(uint32_t pidLooptime)
 void pidResetErrorGyroState(void)
 {
     for (int axis = 0; axis < 3; axis++) {
-        previousGyroIf[axis] = 0.0f;
+        axisPID_I[axis] = 0.0f;
     }
 }
 
@@ -335,56 +329,40 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
         const float errorRate = currentPidSetpoint - gyroRate;       // r - y
 
         // -----calculate P component and add Dynamic Part based on stick input
-        float PTerm = Kp[axis] * errorRate * tpaFactor;
+        axisPID_P[axis] = Kp[axis] * errorRate * tpaFactor;
         if (axis == FD_YAW) {
-            PTerm = ptermYawFilterApplyFn(ptermYawFilter, PTerm);
+            axisPID_P[axis] = ptermYawFilterApplyFn(ptermYawFilter, axisPID_P[axis]);
         }
 
         // -----calculate I component
-        float ITerm = previousGyroIf[axis];
         if (motorMixRange < 1.0f) {
             // Only increase ITerm if motor output is not saturated
-            ITerm += Ki[axis] * errorRate * dT * dynKi * itermAccelerator;
+            axisPID_I[axis] += Ki[axis] * errorRate * dT * dynKi * itermAccelerator;
-            previousGyroIf[axis] = ITerm;
         }
 
         // -----calculate D component
-        if (axis == FD_YAW) {
+        if (axis != FD_YAW) {
-            // no DTerm for yaw axis
-            // -----calculate total PID output
-            axisPIDf[FD_YAW] = PTerm + ITerm;
-#ifdef BLACKBOX
-            axisPID_P[FD_YAW] = PTerm;
-            axisPID_I[FD_YAW] = ITerm;
-            axisPID_D[FD_YAW] = 0;
-#endif
-        } else {
             float dynC = dtermSetpointWeight;
             if (pidProfile->setpointRelaxRatio < 100) {
                 dynC *= MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f);
             }
             const float rD = dynC * currentPidSetpoint - gyroRate;    // cr - y
             // Divide rate change by dT to get differential (ie dr/dt)
-            const float delta = (rD - previousRateError[axis]) / dT;
+            float delta = (rD - previousRateError[axis]) / dT;
             previousRateError[axis] = rD;
 
-            float DTerm = Kd[axis] * delta * tpaFactor;
-            DEBUG_SET(DEBUG_DTERM_FILTER, axis, DTerm);
-
             // apply filters
-            DTerm = dtermNotchFilterApplyFn(dtermFilterNotch[axis], DTerm);
+            delta = dtermNotchFilterApplyFn(dtermFilterNotch[axis], delta);
-            DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm);
+            delta = dtermLpfApplyFn(dtermFilterLpf[axis], delta);
 
-            // -----calculate total PID output
+            axisPID_D[axis] = Kd[axis] * delta * tpaFactor;
-            axisPIDf[axis] = PTerm + ITerm + DTerm;
-#ifdef BLACKBOX
-            axisPID_P[axis] = PTerm;
-            axisPID_I[axis] = ITerm;
-            axisPID_D[axis] = DTerm;
-#endif
         }
 
         // Disable PID control at zero throttle
-        if (!pidStabilisationEnabled) axisPIDf[axis] = 0;
+        if (!pidStabilisationEnabled) {
+            axisPID_P[axis] = 0;
+            axisPID_I[axis] = 0;
+            axisPID_D[axis] = 0;
+        }
     }
 }

src/main/flight/pid.h

@@ -101,8 +101,7 @@ PG_DECLARE(pidConfig_t, pidConfig);
 union rollAndPitchTrims_u;
 void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim);
 
-extern float axisPIDf[3];
+extern float axisPID_P[3], axisPID_I[3], axisPID_D[3];
-extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
 bool airmodeWasActivated;
 extern uint32_t targetPidLooptime;
 

src/main/flight/servos.c

@@ -379,9 +379,9 @@ STATIC_UNIT_TESTED void servoMixer(void)
         input[INPUT_STABILIZED_YAW] = rcCommand[YAW];
     } else {
         // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
-        input[INPUT_STABILIZED_ROLL] = axisPIDf[ROLL] * PID_SERVO_MIXER_SCALING;
+        input[INPUT_STABILIZED_ROLL] = (axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL]) * PID_SERVO_MIXER_SCALING;
-        input[INPUT_STABILIZED_PITCH] = axisPIDf[PITCH] * PID_SERVO_MIXER_SCALING;
+        input[INPUT_STABILIZED_PITCH] = (axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) * PID_SERVO_MIXER_SCALING;
-        input[INPUT_STABILIZED_YAW] = axisPIDf[YAW] * PID_SERVO_MIXER_SCALING;
+        input[INPUT_STABILIZED_YAW] = (axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) * PID_SERVO_MIXER_SCALING;
 
         // Reverse yaw servo when inverted in 3D mode
         if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->midrc)) {