Improved CRSF flight mode reporting.

src/main/telemetry/crsf.c

@@ -267,19 +267,39 @@ void crsfFrameFlightMode(sbuf_t *dst)
     sbufWriteU8(dst, 0);
     sbufWriteU8(dst, CRSF_FRAMETYPE_FLIGHT_MODE);
 
-    // use same logic as OSD, so telemetry displays same flight text as OSD
+    // Acro is the default mode
     const char *flightMode = "ACRO";
-    if (airmodeIsEnabled()) {
-        flightMode = "AIR";
-    }
+
+    // Modes that are only relevant when disarmed
+    if (!ARMING_FLAG(ARMED) && isArmingDisabled()) {
+        flightMode = "!ERR";
+    } else
+
+#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)) {
-        flightMode = "!FS";
+        flightMode = "!FS!";
+    } else if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
+        flightMode = "RTH";
+    } else if (FLIGHT_MODE(PASSTHRU_MODE)) {
+        flightMode = "MANU";
     } else if (FLIGHT_MODE(ANGLE_MODE)) {
         flightMode = "STAB";
     } else if (FLIGHT_MODE(HORIZON_MODE)) {
         flightMode = "HOR";
+    } else if (airmodeIsEnabled()) {
+        flightMode = "AIR";
     }
+
     sbufWriteString(dst, flightMode);
+    if (!ARMING_FLAG(ARMED)) {
+        sbufWriteU8(dst, '*');
+    }
     sbufWriteU8(dst, '\0');     // zero-terminate string
     // write in the frame length
     *lengthPtr = sbufPtr(dst) - lengthPtr;

src/test/unit/telemetry_crsf_unittest.cc

@@ -216,9 +216,30 @@ TEST(TelemetryCrsfTest, TestFlightMode)
 {
     uint8_t frame[CRSF_FRAME_SIZE_MAX];
 
-    // nothing set, so ACRO mode
+    ENABLE_STATE(GPS_FIX);
+    ENABLE_STATE(GPS_FIX_HOME);
+
     airMode = false;
+
+    DISABLE_ARMING_FLAG(ARMED);
+
+    // nothing set, so ACRO mode
     int frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE);
+    EXPECT_EQ(6 + FRAME_HEADER_FOOTER_LEN, frameLen);
+    EXPECT_EQ(CRSF_SYNC_BYTE, frame[0]); // address
+    EXPECT_EQ(8, frame[1]); // length
+    EXPECT_EQ(0x21, frame[2]); // type
+    EXPECT_EQ('A', frame[3]);
+    EXPECT_EQ('C', frame[4]);
+    EXPECT_EQ('R', frame[5]);
+    EXPECT_EQ('O', frame[6]);
+    EXPECT_EQ('*', frame[7]);
+    EXPECT_EQ(0, frame[8]);
+    EXPECT_EQ(crfsCrc(frame, frameLen), frame[9]);
+
+    ENABLE_ARMING_FLAG(ARMED);
+
+    frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE);
     EXPECT_EQ(5 + FRAME_HEADER_FOOTER_LEN, frameLen);
     EXPECT_EQ(CRSF_SYNC_BYTE, frame[0]); // address
     EXPECT_EQ(7, frame[1]); // length
@@ -230,7 +251,6 @@ TEST(TelemetryCrsfTest, TestFlightMode)
     EXPECT_EQ(0, frame[7]);
     EXPECT_EQ(crfsCrc(frame, frameLen), frame[8]);
 
-
     enableFlightMode(ANGLE_MODE);
     EXPECT_EQ(ANGLE_MODE, FLIGHT_MODE(ANGLE_MODE));
     frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE);