From c0964cbb2cf15d878fa6b57efe45ef2a3a96e9ad Mon Sep 17 00:00:00 2001
From: Tim Eckel <tim@leethost.com>
Date: Sun, 25 Nov 2018 04:06:15 -0500
Subject: [PATCH] Update Crossfire (crsf) telemetry flight modes to match OSD
 (#3912)

Refactor OSD flight mode logic

As of 2.0.0 POSHOLD always includes ALTHOLD, so remove that unused legacy logic.  Also reformatted as per coding style.

First run at enhancing Crossfire telemetry
---
 src/main/telemetry/crsf.c | 48 +++++++++++++++++++++++++++++++--------
 1 file changed, 38 insertions(+), 10 deletions(-)

diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index fc369e8ac1..a24779b82c 100755
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -39,6 +39,7 @@
 
 #include "fc/config.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "flight/imu.h"
@@ -228,21 +229,49 @@ char[]      Flight mode ( Null­terminated string )
 */
 void crsfFrameFlightMode(sbuf_t *dst)
 {
-    // just do Angle for the moment as a placeholder
+    // just do "OK" for the moment as a placeholder
     // write zero for frame length, since we don't know it yet
     uint8_t *lengthPtr = sbufPtr(dst);
     sbufWriteU8(dst, 0);
     crsfSerialize8(dst, CRSF_FRAMETYPE_FLIGHT_MODE);
 
-    // use same logic as OSD, so telemetry displays same flight text as OSD
-    const char *flightMode = "ACRO";
-    if (FLIGHT_MODE(FAILSAFE_MODE)) {
-        flightMode = "!FS";
-    } else if (FLIGHT_MODE(ANGLE_MODE)) {
-        flightMode = "ANGL";
-    } else if (FLIGHT_MODE(HORIZON_MODE)) {
-        flightMode = "HOR";
+    // use same logic as OSD, so telemetry displays same flight text as OSD when armed
+    const char *flightMode = "OK";
+    if (ARMING_FLAG(ARMED)) {
+        if (isAirmodeActive()) {
+            flightMode = "AIR";
+        } else {
+            flightMode = "ACRO";
+        }
+        if (FLIGHT_MODE(FAILSAFE_MODE)) {
+            flightMode = "!FS!";
+        } else if (FLIGHT_MODE(MANUAL_MODE)) {
+            flightMode = "MANU";
+        } else if (FLIGHT_MODE(NAV_RTH_MODE)) {
+            flightMode = "RTH";
+        } else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) {
+            flightMode = "HOLD";
+        } else if (FLIGHT_MODE(NAV_CRUISE_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
+            flightMode = "3CRS";
+        } else if (FLIGHT_MODE(NAV_CRUISE_MODE)) {
+            flightMode = "CRS";
+        } else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) {
+            flightMode = "AH";
+        } else if (FLIGHT_MODE(NAV_WP_MODE)) {
+            flightMode = "WP";
+        } else if (FLIGHT_MODE(ANGLE_MODE)) {
+            flightMode = "ANGL";
+        } else if (FLIGHT_MODE(HORIZON_MODE)) {
+            flightMode = "HOR";
+        }
+#ifdef USE_GPS
+    } else if (feature(FEATURE_GPS) && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) {
+        flightMode = "WAIT"; // Waiting for GPS lock
+#endif
+    } else if (isArmingDisabled()) {
+        flightMode = "!ERR";
     }
+
     crsfSerializeData(dst, (const uint8_t*)flightMode, strlen(flightMode));
     crsfSerialize8(dst, 0); // zero terminator for string
     // write in the length
@@ -256,7 +285,6 @@ void crsfFrameFlightMode(sbuf_t *dst)
 static uint8_t crsfScheduleCount;
 static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
 
-
 static void processCrsf(void)
 {
     static uint8_t crsfScheduleIndex = 0;