Added disabling of GPS_RESCUE when 3D is enabled.

src/main/fc/config.c

@@ -179,8 +179,12 @@ static void validateAndFixConfig(void)
         currentPidProfile->dterm_notch_hz = 0;
     }
 
-    if ((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)) {
-        motorConfigMutable()->mincommand = 1000;
+    if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
+        featureClear(FEATURE_3D);
+
+        if (motorConfig()->mincommand < 1000) {
+            motorConfigMutable()->mincommand = 1000;
+        }
     }
 
     if ((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_STANDARD) && (motorConfig()->dev.motorPwmRate > BRUSHLESS_MOTORS_PWM_RATE)) {
@@ -232,14 +236,14 @@ static void validateAndFixConfig(void)
 #endif // USE_SOFTSPI
 
 #if defined(USE_ADC)
-    if (feature(FEATURE_RSSI_ADC)) {
+    if (featureConfigured(FEATURE_RSSI_ADC)) {
         rxConfigMutable()->rssi_channel = 0;
         rxConfigMutable()->rssi_src_frame_errors = false;
     } else
 #endif
     if (rxConfigMutable()->rssi_channel
 #if defined(USE_PWM) || defined(USE_PPM)
-        || feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)
+        || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_PARALLEL_PWM)
 #endif
         ) {
         rxConfigMutable()->rssi_src_frame_errors = false;
@@ -267,6 +271,29 @@ static void validateAndFixConfig(void)
         pgResetFn_serialConfig(serialConfigMutable());
     }
 
+    if (
+#if defined(USE_GPS)
+        !findSerialPortConfig(FUNCTION_GPS) &&
+#endif
+        true) {
+        featureClear(FEATURE_GPS);
+    }
+
+    if (
+        featureConfigured(FEATURE_3D) || !featureConfigured(FEATURE_GPS)
+#if !defined(USE_GPS) || !defined(USE_GPS_RESCUE)
+        || true
+#endif
+        ) {
+        if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE) {
+            failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT;
+        }
+
+        if (isModeActivationConditionPresent(BOXGPSRESCUE)) {
+            removeModeActivationCondition(BOXGPSRESCUE);
+        }
+    }
+
 #if defined(USE_ESC_SENSOR)
     if (!findSerialPortConfig(FUNCTION_ESC_SENSOR)) {
         featureClear(FEATURE_ESC_SENSOR);
@@ -288,10 +315,6 @@ static void validateAndFixConfig(void)
     featureClear(FEATURE_SOFTSERIAL);
 #endif
 
-#ifndef USE_GPS
-    featureClear(FEATURE_GPS);
-#endif
-
 #ifndef USE_RANGEFINDER
     featureClear(FEATURE_RANGEFINDER);
 #endif

src/main/fc/fc_init.c

@@ -352,7 +352,6 @@ void init(void)
         idlePulse = flight3DConfig()->neutral3d;
     }
     if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
-        featureClear(FEATURE_3D);
         idlePulse = 0; // brushed motors
     }
     /* Motors needs to be initialized soon as posible because hardware initialization

src/main/fc/rc_modes.c

@@ -125,3 +125,27 @@ bool isModeActivationConditionPresent(boxId_e modeId)
 
     return false;
 }
+
+void removeModeActivationCondition(const boxId_e modeId)
+{
+    unsigned offset = 0;
+    for (unsigned i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
+        modeActivationCondition_t *mac = modeActivationConditionsMutable(i);
+
+        if (mac->modeId == modeId && !offset) {
+            offset++;
+        }
+
+        if (offset) {
+            while (i + offset < MAX_MODE_ACTIVATION_CONDITION_COUNT && modeActivationConditions(i + offset)->modeId == modeId) {
+                offset++;
+            }
+
+            if (i + offset < MAX_MODE_ACTIVATION_CONDITION_COUNT) {
+                memcpy(mac, modeActivationConditions(i + offset), sizeof(modeActivationCondition_t));
+            } else {
+                memset(mac, 0, sizeof(modeActivationCondition_t));
+            }
+        }
+    }
+}

src/main/fc/rc_modes.h

@@ -137,3 +137,4 @@ bool isAntiGravityModeActive(void);
 bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
 void updateActivatedModes(void);
 bool isModeActivationConditionPresent(boxId_e modeId);
+void removeModeActivationCondition(boxId_e modeId);

src/main/flight/gps_rescue.c

@@ -34,6 +34,7 @@
 #include "fc/config.h"
 #include "fc/fc_core.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "flight/failsafe.h"
@@ -438,5 +439,9 @@ float gpsRescueGetThrottle(void)
     return commandedThrottle;
 }
 
+bool gpsRescueIsConfigured(void)
+{
+    return failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE || isModeActivationConditionPresent(BOXGPSRESCUE);
+}
 #endif
 

src/main/flight/gps_rescue.h

@@ -111,3 +111,4 @@ void rescueAttainPosition(void);
 
 float gpsRescueGetYawRate(void);
 float gpsRescueGetThrottle(void);
+bool gpsRescueIsConfigured(void);

src/main/interface/msp_box.c

@@ -197,7 +197,11 @@ void initActiveBoxIds(void)
     if (feature(FEATURE_GPS)) {
         BME(BOXGPSHOME);
         BME(BOXGPSHOLD);
-        BME(BOXGPSRESCUE);
+#ifdef USE_GPS_RESCUE
+        if (!feature(FEATURE_3D)) {
+            BME(BOXGPSRESCUE);
+        }
+#endif
         BME(BOXBEEPGPSCOUNT);
     }
 #endif

src/main/io/gps.c

@@ -283,7 +283,6 @@ void gpsInit(void)
 
     serialPortConfig_t *gpsPortConfig = findSerialPortConfig(FUNCTION_GPS);
     if (!gpsPortConfig) {
-        featureClear(FEATURE_GPS);
         return;
     }
 
@@ -305,7 +304,6 @@ void gpsInit(void)
     // no callback - buffer will be consumed in gpsUpdate()
     gpsPort = openSerialPort(gpsPortConfig->identifier, FUNCTION_GPS, NULL, NULL, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex], mode, SERIAL_NOT_INVERTED);
     if (!gpsPort) {
-        featureClear(FEATURE_GPS);
         return;
     }
 
@@ -532,7 +530,9 @@ void gpsUpdate(timeUs_t currentTimeUs)
         updateGpsIndicator(currentTimeUs);
     }
 #if defined(USE_GPS_RESCUE)
-    updateGPSRescueState();
+    if (gpsRescueIsConfigured()) {
+        updateGPSRescueState();
+    }
 #endif
 }