CRSF Flight Mode Sensor Value update (betaflight#13854)

* CRSF Flight Mode Sensor Value update

* Fix FS FM

* Add ? for GPS not available

* Need to check define

* better name

* Update mode and test unit

* Fixes review CT

* Fixes review JimB40

* Apply ctz comment

* Update isAllowArmingWithoutFix

* Fix fs/resc sw

* Add required sats

* Update comments

src/main/fc/runtime_config.h

@@ -132,4 +132,3 @@ void sensorsSet(uint32_t mask);
 void sensorsClear(uint32_t mask);
 uint32_t sensorsMask(void);
 
-void mwDisarm(void);

src/main/flight/failsafe.c

@@ -425,8 +425,8 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                 break;
         }
 
-    DEBUG_SET(DEBUG_FAILSAFE, 0, failsafeState.boxFailsafeSwitchWasOn);
-    DEBUG_SET(DEBUG_FAILSAFE, 3, failsafeState.phase);
+        DEBUG_SET(DEBUG_FAILSAFE, 0, failsafeState.boxFailsafeSwitchWasOn);
+        DEBUG_SET(DEBUG_FAILSAFE, 3, failsafeState.phase);
 
     } while (reprocessState);
 

src/main/osd/osd_elements.c

@@ -1061,7 +1061,7 @@ static void osdElementFlymode(osdElementParms_t *element)
     } else if (FLIGHT_MODE(ANGLE_MODE)) {
         strcpy(element->buff, "ANGL");
     } else if (FLIGHT_MODE(ALT_HOLD_MODE)) {
-        strcpy(element->buff, "ALTH ");
+        strcpy(element->buff, "ALTH");
     } else if (FLIGHT_MODE(HORIZON_MODE)) {
         strcpy(element->buff, "HOR ");
     } else if (IS_RC_MODE_ACTIVE(BOXACROTRAINER)) {

src/main/telemetry/crsf.c

@@ -48,6 +48,7 @@
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
+#include "flight/gps_rescue.h"
 #include "flight/imu.h"
 #include "flight/position.h"
 
@@ -391,33 +392,36 @@ void crsfFrameFlightMode(sbuf_t *dst)
     // Acro is the default mode
     const char *flightMode = "ACRO";
 
-#if defined(USE_GPS)
-    if (!ARMING_FLAG(ARMED) && featureIsEnabled(FEATURE_GPS) && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) {
-        flightMode = "WAIT"; // Waiting for GPS lock
-    } else
-#endif
-
     // Flight modes in decreasing order of importance
-    if (FLIGHT_MODE(FAILSAFE_MODE)) {
+    if (FLIGHT_MODE(FAILSAFE_MODE) || IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
         flightMode = "!FS!";
-    } else if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
+    } else if (FLIGHT_MODE(GPS_RESCUE_MODE) || IS_RC_MODE_ACTIVE(BOXGPSRESCUE)) {
         flightMode = "RTH";
     } else if (FLIGHT_MODE(PASSTHRU_MODE)) {
         flightMode = "MANU";
     } else if (FLIGHT_MODE(ANGLE_MODE)) {
-        flightMode = "STAB";
+        flightMode = "ANGL";
     } else if (FLIGHT_MODE(ALT_HOLD_MODE)) {
-        flightMode = "ALTH ";
+        flightMode = "ALTH";
     } else if (FLIGHT_MODE(HORIZON_MODE)) {
         flightMode = "HOR";
     } else if (airmodeIsEnabled()) {
         flightMode = "AIR";
     }
 
     sbufWriteString(dst, flightMode);
-    if (!ARMING_FLAG(ARMED)) {
-        sbufWriteU8(dst, '*');
+
+    if (!ARMING_FLAG(ARMED) && !FLIGHT_MODE(FAILSAFE_MODE)) {
+        // * = ready to arm
+        // ! = arming disabled
+        // ? = GPS rescue disabled
+        bool isGpsRescueDisabled = false;
+#ifdef USE_GPS
+        isGpsRescueDisabled = featureIsEnabled(FEATURE_GPS) && gpsRescueIsConfigured() && gpsSol.numSat < gpsRescueConfig()->minSats && !STATE(GPS_FIX);
+#endif
+        sbufWriteU8(dst, isArmingDisabled() ? '!' : isGpsRescueDisabled ? '?' : '*');
     }
+
     sbufWriteU8(dst, '\0');     // zero-terminate string
     // write in the frame length
     *lengthPtr = sbufPtr(dst) - lengthPtr;

src/test/unit/rx_spi_expresslrs_telemetry_unittest.cc

@@ -44,6 +44,8 @@ extern "C" {
     #include "telemetry/msp_shared.h"
     #include "rx/crsf_protocol.h"
     #include "rx/expresslrs_telemetry.h"
+    #include "fc/rc_modes.h"
+    #include "flight/gps_rescue.h"
     #include "flight/imu.h"
 
     #include "sensors/battery.h"
@@ -70,6 +72,7 @@ extern "C" {
 
     PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
     PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
+    PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0);
 }
 
 #include "unittest_macros.h"
@@ -209,10 +212,10 @@ TEST(RxSpiExpressLrsTelemetryUnitTest, TestFlightMode)
     getNextTelemetryPayload(&payloadSize, &payload);
     EXPECT_EQ(currentPayloadIndex, 0);
 
-    EXPECT_EQ('W', payload[3]);
-    EXPECT_EQ('A', payload[4]);
-    EXPECT_EQ('I', payload[5]);
-    EXPECT_EQ('T', payload[6]);
+    EXPECT_EQ('A', payload[3]);
+    EXPECT_EQ('C', payload[4]);
+    EXPECT_EQ('R', payload[5]);
+    EXPECT_EQ('O', payload[6]);
     EXPECT_EQ('*', payload[7]);
     EXPECT_EQ(0, payload[8]);
 
@@ -464,4 +467,9 @@ extern "C" {
 
     timeUs_t rxFrameTimeUs(void) { return 0; }
 
+    bool IS_RC_MODE_ACTIVE(boxId_e) { return false; }
+
+    int getArmingDisableFlags(void) { return 0; }
+
+    bool gpsRescueIsConfigured(void) { return false; }
 }

src/test/unit/telemetry_crsf_msp_unittest.cc

@@ -44,7 +44,9 @@ extern "C" {
     #include "drivers/system.h"
 
     #include "fc/runtime_config.h"
+    #include "fc/rc_modes.h"
     #include "config/config.h"
+    #include "flight/gps_rescue.h"
     #include "flight/imu.h"
 
     #include "io/serial.h"
@@ -85,6 +87,7 @@ extern "C" {
     PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
     PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0);
     PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG,0);
+    PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0);
 
     extern bool crsfFrameDone;
     extern crsfFrame_t crsfFrame;
@@ -324,4 +327,8 @@ extern "C" {
     }
 
     timeUs_t rxFrameTimeUs(void) { return 0; }
+
+    bool IS_RC_MODE_ACTIVE(boxId_e) { return false; }
+
+    bool gpsRescueIsConfigured(void) { return false; }
 }

src/test/unit/telemetry_crsf_unittest.cc

@@ -43,8 +43,10 @@ extern "C" {
 
     #include "config/config.h"
     #include "fc/runtime_config.h"
+    #include "fc/rc_modes.h"
 
     #include "flight/pid.h"
+    #include "flight/gps_rescue.h"
     #include "flight/imu.h"
 
     #include "io/gps.h"
@@ -79,6 +81,7 @@ extern "C" {
     PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
     PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0);
     PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
+    PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0);
 }
 
 #include "unittest_macros.h"
@@ -264,10 +267,10 @@ TEST(TelemetryCrsfTest, TestFlightMode)
     EXPECT_EQ(CRSF_SYNC_BYTE, frame[0]); // address
     EXPECT_EQ(7, frame[1]); // length
     EXPECT_EQ(0x21, frame[2]); // type
-    EXPECT_EQ('S', frame[3]);
-    EXPECT_EQ('T', frame[4]);
-    EXPECT_EQ('A', frame[5]);
-    EXPECT_EQ('B', frame[6]);
+    EXPECT_EQ('A', frame[3]);
+    EXPECT_EQ('N', frame[4]);
+    EXPECT_EQ('G', frame[5]);
+    EXPECT_EQ('L', frame[6]);
     EXPECT_EQ(0, frame[7]);
     EXPECT_EQ(crfsCrc(frame, frameLen), frame[8]);
 
@@ -389,4 +392,6 @@ bool handleMspFrame(uint8_t *, uint8_t, uint8_t *)  { return false; }
 bool isBatteryVoltageConfigured(void) { return true; }
 bool isAmperageConfigured(void) { return true; }
 timeUs_t rxFrameTimeUs(void) { return 0; }
+bool IS_RC_MODE_ACTIVE(boxId_e) { return false; }
+bool gpsRescueIsConfigured(void) { return false; }
 }