AP_Frsky_Telem: added passthrough frame 0x500B for height above terrain

libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp

@@ -8,6 +8,7 @@
 #include <AP_Notify/AP_Notify.h>
 #include <AP_RangeFinder/AP_RangeFinder.h>
 #include <AP_RPM/AP_RPM.h>
+#include <AP_Terrain/AP_Terrain.h>
 #include <GCS_MAVLink/GCS.h>
 
 #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
@@ -112,6 +113,7 @@ void AP_Frsky_SPort_Passthrough::setup_wfq_scheduler(void)
     set_scheduler_entry(BATT_1, 1300, 500);     // 0x5003 Battery 1 status
     set_scheduler_entry(PARAM, 1700, 1000);     // 0x5007 parameters
     set_scheduler_entry(RPM, 300, 330);         // 0x500A rpm sensors 1 and 2
+    set_scheduler_entry(TERRAIN, 700, 500);     // 0x500B terrain data
     set_scheduler_entry(UDATA, 5000, 200);      // user data
 #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
     set_scheduler_entry(MAV, 35, 25);           // mavlite
@@ -200,6 +202,15 @@ bool AP_Frsky_SPort_Passthrough::is_packet_ready(uint8_t idx, bool queue_empty)
             packet_ready = rpm->num_sensors() > 0;
         }
         break;
+    case TERRAIN:
+        {
+            packet_ready = false;
+#if AP_TERRAIN_AVAILABLE
+            const AP_Terrain &terrain = AP::terrain();
+            packet_ready = terrain.enabled();
+#endif
+        }
+        break;
     case UDATA:
         // when using fport user data is sent by scheduler
         // when using sport user data is sent responding to custom polling
@@ -269,6 +280,9 @@ void AP_Frsky_SPort_Passthrough::process_packet(uint8_t idx)
     case RPM: // 0x500A rpm sensors 1 and 2
         send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+0x0A, calc_rpm());
         break;
+    case TERRAIN: // 0x500B terrain data
+        send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+0x0B, calc_terrain());
+        break;
     case UDATA: // user data
         {
             WITH_SEMAPHORE(_sport_push_buffer.sem);
@@ -616,6 +630,27 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_rpm(void)
     return value;
 }
 
+/*
+ * prepare terrain data
+ * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
+ */
+uint32_t AP_Frsky_SPort_Passthrough::calc_terrain(void)
+{
+    uint32_t value = 0;
+#if AP_TERRAIN_AVAILABLE
+    AP_Terrain &terrain = AP::terrain();
+    if (!terrain.enabled()) {
+        return value;
+    }
+    float height_above_terrain;
+    if (terrain.height_above_terrain(height_above_terrain, true)) {
+        // vehicle height above terrain
+        value |= prep_number(roundf(height_above_terrain * 10), 3, 2);
+    }
+#endif
+    return value;
+}
+
 /*
   fetch Sport data for an external transport, such as FPort or crossfire
   Note: we need to create a packet array with unique packet types

libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h

@@ -72,6 +72,7 @@ public:
 #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
         MAV =           13,  // mavlite
 #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
+        TERRAIN =       14, // 0x500B terrain data
         WFQ_LAST_ITEM       // must be last
     };
 
@@ -99,6 +100,7 @@ private:
     uint32_t calc_velandyaw(void);
     uint32_t calc_attiandrng(void);
     uint32_t calc_rpm(void);
+    uint32_t calc_terrain(void);
 
     // use_external_data is set when this library will
     // be providing data to another transport, such as FPort