Merge pull request #11104 from iNavFlight/MrD_Update-SmartPort-modes-to-include-new-modes

Update SmartPort telemetry to reflect latest flight modes

Author: sensei-hacker

docs/Telemetry.md

@@ -102,12 +102,14 @@ The following sensors are transmitted
 * **VSpd** : vertical speed, unit is cm/s.
 * **Hdg** : heading, North is 0°, South is 180°.
 * **AccX,Y,Z** : accelerometer values (not sent if `frsky_pitch_roll = ON`).
-* **470** : flight mode, sent as 5 digits. Number is sent as **ABCDE** detailed below. The numbers are additives (for example: if digit C is 6, it means both position hold and altitude hold are active) :
-  * **A** : 1 = flaperon mode, 2 = auto tune mode, 4 = failsafe mode
-  * **B** : 1 = return to home, 2 = waypoint mode, 4 = headfree mode
-  * **C** : 1 = heading hold, 2 = altitude hold, 4 = position hold
-  * **D** : 1 = angle mode, 2 = horizon mode, 4 = passthru mode
-  * **E** : 1 = ok to arm, 2 = arming is prevented, 4 = armed
+* **470** : flight mode, sent as 7 digits. Number is sent as **ABCDEFG** detailed below. The numbers are additives (for example: if digit C is 6, it means both position hold and altitude hold are active) :
+  * **A** : 1 = WRTH mode, 2 = Angle hold mode
+  * **B** : 1 = Fixed Wing Auto-land, 2 = Turtle mode, 4 = Geofence action mode, 8 = Loiter mode
+  * **C** : 1 = flaperon mode, 2 = auto tune mode, 4 = failsafe mode
+  * **D** : 1 = return to home, 2 = waypoint mode, 4 = headfree mode, 8 = Course Hold
+  * **E** : 1 = heading hold, 2 = altitude hold, 4 = position hold
+  * **F** : 1 = angle mode, 2 = horizon mode, 4 = passthru mode
+  * **G** : 1 = ok to arm, 2 = arming is prevented, 4 = armed
 
   _NOTE_ This sensor used to be **Tmp1**. The ID has been reassigned in INAV 8.0. The old ID of **Tmp1** can still be used, by using `set frsky_use_legacy_gps_mode_sensor_ids = ON`. This is deprecated and will be removed in INAV 10.0. All tools and scripts using the old IDs should be updated to use the new ID.
 * **480** : GPS lock status, accuracy, home reset trigger, and number of satellites. Number is sent as **ABCD** detailed below. Typical minimum GPS 3D lock value is 3906 (GPS locked and home fixed, HDOP highest accuracy, 6 satellites).

src/main/io/osd.c

@@ -2565,7 +2565,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 p = "MANU";
 #ifdef USE_GEOZONE
             else if (FLIGHT_MODE(NAV_SEND_TO) && !FLIGHT_MODE(NAV_WP_MODE))
-                p = "GEO";
+                p = "GEO ";
 #endif
             else if (FLIGHT_MODE(TURTLE_MODE))
                 p = "TURT";

src/main/telemetry/crsf.c

@@ -349,6 +349,8 @@ static void crsfFrameFlightMode(sbuf_t *dst)
     sbufWriteU8(dst, 0);
     crsfSerialize8(dst, CRSF_FRAMETYPE_FLIGHT_MODE);
 
+    static uint8_t hrstSent = 0;
+
     // use same logic as OSD, so telemetry displays same flight text as OSD when armed
     const char *flightMode = "OK";
     if (ARMING_FLAG(ARMED)) {
@@ -359,7 +361,10 @@ static void crsfFrameFlightMode(sbuf_t *dst)
         } else
 #endif
         if (FLIGHT_MODE(FAILSAFE_MODE)) {
-            flightMode = "!FS!";          
+            flightMode = "!FS!";
+        } else if (IS_RC_MODE_ACTIVE(BOXHOMERESET) && hrstSent < 4 && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE)) {
+            flightMode = "HRST";
+            hrstSent++;
         } else if (FLIGHT_MODE(MANUAL_MODE)) {
             flightMode = "MANU";
 #ifdef USE_GEOZONE
@@ -397,6 +402,9 @@ static void crsfFrameFlightMode(sbuf_t *dst)
         flightMode = "!ERR";
     }
 
+    if (!IS_RC_MODE_ACTIVE(BOXHOMERESET) && hrstSent > 0)
+        hrstSent = 0;
+
     crsfSerializeData(dst, (const uint8_t*)flightMode, strlen(flightMode));
     crsfSerialize8(dst, 0); // zero terminator for string
     // write in the length

src/main/telemetry/smartport.c

@@ -162,36 +162,36 @@ static smartPortWriteFrameFn *smartPortWriteFrame;
 static bool smartPortMspReplyPending = false;
 #endif
 
-static uint16_t frskyGetFlightMode(void)
+static uint32_t frskyGetFlightMode(void)
 {
-    uint16_t tmpi = 0;
+    uint32_t tmpi = 0;
 
-    // ones column
+    // ones column (G)
     if (!isArmingDisabled())
         tmpi += 1;
     else
         tmpi += 2;
     if (ARMING_FLAG(ARMED))
         tmpi += 4;
 
-    // tens column
+    // tens column (F)
     if (FLIGHT_MODE(ANGLE_MODE))
         tmpi += 10;
     if (FLIGHT_MODE(HORIZON_MODE))
         tmpi += 20;
     if (FLIGHT_MODE(MANUAL_MODE))
         tmpi += 40;
 
-    // hundreds column
+    // hundreds column (E)
     if (FLIGHT_MODE(HEADING_MODE))
         tmpi += 100;
     if (FLIGHT_MODE(NAV_ALTHOLD_MODE))
         tmpi += 200;
-    if (FLIGHT_MODE(NAV_POSHOLD_MODE))
+    if (FLIGHT_MODE(NAV_POSHOLD_MODE) && !STATE(AIRPLANE))
         tmpi += 400;
 
-    // thousands column
-    if (FLIGHT_MODE(NAV_RTH_MODE))
+    // thousands column (D)
+    if (FLIGHT_MODE(NAV_RTH_MODE) && !isWaypointMissionRTHActive())
         tmpi += 1000;
     if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) // intentionally out of order and 'else-ifs' to prevent column overflow
         tmpi += 8000;
@@ -200,14 +200,30 @@ static uint16_t frskyGetFlightMode(void)
     else if (FLIGHT_MODE(HEADFREE_MODE))
         tmpi += 4000;
 
-    // ten thousands column
+    // ten thousands column (C)
     if (FLIGHT_MODE(FLAPERON))
         tmpi += 10000;
     if (FLIGHT_MODE(FAILSAFE_MODE))
         tmpi += 40000;
     else if (FLIGHT_MODE(AUTO_TUNE)) // intentionally reverse order and 'else-if' to prevent 16-bit overflow
         tmpi += 20000;
 
+    // hundred thousands column (B)
+    if (FLIGHT_MODE(NAV_FW_AUTOLAND))
+        tmpi += 100000;
+    if (FLIGHT_MODE(TURTLE_MODE))
+        tmpi += 200000;
+    else if (FLIGHT_MODE(NAV_POSHOLD_MODE) && STATE(AIRPLANE))
+        tmpi += 800000;
+    if (FLIGHT_MODE(NAV_SEND_TO))
+        tmpi += 400000;
+
+    // million column (A)
+    if (FLIGHT_MODE(NAV_RTH_MODE) && isWaypointMissionRTHActive())
+        tmpi += 1000000;
+    if (FLIGHT_MODE(ANGLEHOLD_MODE))
+        tmpi += 2000000;
+
     return tmpi;
 }