AP_Frsky_Telem: added frame 0x500D for waypoint data

libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp

@@ -421,7 +421,7 @@ uint16_t AP_Frsky_SPort::prep_number(int32_t number, uint8_t digits, uint8_t pow
             res = abs_number<<1;
         } else if (abs_number < 10240) {
             res = ((uint16_t)roundf(abs_number * 0.1f)<<1)|0x1;
-        } else { // transmit max possible value (0x3FF x 10^1 = 10240)
+        } else { // transmit max possible value (0x3FF x 10^1 = 10230)
             res = 0x7FF;
         }
         if (number < 0) { // if number is negative, add sign bit in front
@@ -436,7 +436,7 @@ uint16_t AP_Frsky_SPort::prep_number(int32_t number, uint8_t digits, uint8_t pow
             res = ((uint16_t)roundf(abs_number * 0.01f)<<2)|0x2;
         } else if (abs_number < 1024000) {
             res = ((uint16_t)roundf(abs_number * 0.001f)<<2)|0x3;
-        } else { // transmit max possible value (0x3FF x 10^3 = 127000)
+        } else { // transmit max possible value (0x3FF x 10^3 = 1023000)
             res = 0xFFF;
         }
         if (number < 0) { // if number is negative, add sign bit in front

libraries/AP_Frsky_Telem/AP_Frsky_SPort.h

@@ -29,9 +29,9 @@ protected:
 
     void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data);
 
-    struct PACKED {
-        bool send_latitude; // sizeof(bool) = 4 ?
-        bool send_airspeed; // toggles 0x5005 between airspeed and groundspeed
+    struct {
+        bool send_latitude;
+        bool send_airspeed;     // toggles 0x5005 between airspeed and groundspeed
         uint32_t gps_lng_sample;
         uint8_t new_byte;
     } _passthrough;

libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp

@@ -10,6 +10,7 @@
 #include <AP_RPM/AP_RPM.h>
 #include <AP_Terrain/AP_Terrain.h>
 #include <AC_Fence/AC_Fence.h>
+#include <AP_Vehicle/AP_Vehicle.h>
 #include <GCS_MAVLink/GCS.h>
 #if APM_BUILD_TYPE(APM_BUILD_Rover)
 #include <AP_WindVane/AP_WindVane.h>
@@ -75,6 +76,11 @@ for FrSky SPort Passthrough
 #define WIND_SPEED_OFFSET           7
 #define WIND_APPARENT_ANGLE_OFFSET  15
 #define WIND_APPARENT_SPEED_OFFSET  23
+// for waypoint data
+#define WP_NUMBER_LIMIT             2047
+#define WP_DISTANCE_LIMIT           1023000
+#define WP_DISTANCE_OFFSET          11
+#define WP_BEARING_OFFSET           23
 
 extern const AP_HAL::HAL& hal;
 
@@ -131,6 +137,7 @@ void AP_Frsky_SPort_Passthrough::setup_wfq_scheduler(void)
     set_scheduler_entry(RPM, 300, 330);         // 0x500A rpm sensors 1 and 2
     set_scheduler_entry(TERRAIN, 700, 500);     // 0x500B terrain data
     set_scheduler_entry(WIND, 700, 500);        // 0x500C wind data
+    set_scheduler_entry(WAYPOINT, 750, 500);    // 0x500D waypoint data
     set_scheduler_entry(UDATA, 5000, 200);      // user data
 
     // initialize default sport sensor ID
@@ -248,6 +255,12 @@ bool AP_Frsky_SPort_Passthrough::is_packet_ready(uint8_t idx, bool queue_empty)
     }
 #endif
         break;
+    case WAYPOINT:
+        {
+            const AP_Mission *mission = AP::mission();
+            packet_ready = mission != nullptr && mission->get_current_nav_index() > 0;
+        }
+        break;
     case UDATA:
         // when using fport user data is sent by scheduler
         // when using sport user data is sent responding to custom polling
@@ -323,6 +336,9 @@ void AP_Frsky_SPort_Passthrough::process_packet(uint8_t idx)
     case WIND: // 0x500C terrain data
         send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+0x0C, calc_wind());
         break;
+    case WAYPOINT: // 0x500D waypoint data
+        send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+0x0D, calc_waypoint());
+        break;
     case UDATA: // user data
         {
             WITH_SEMAPHORE(_sport_push_buffer.sem);
@@ -754,6 +770,33 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_wind(void)
 #endif
     return value;
 }
+ /*
+ * prepare waypoint data
+ * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
+ */
+uint32_t AP_Frsky_SPort_Passthrough::calc_waypoint(void)
+{
+    const AP_Mission *mission = AP::mission();
+    const AP_Vehicle *vehicle = AP::vehicle();
+    if (mission == nullptr || vehicle == nullptr) {
+        return 0U;
+    }
+    float wp_distance;
+    if (!vehicle->get_wp_distance_m(wp_distance)) {
+        return 0U;
+    }
+    float angle;
+    if (!vehicle->get_wp_bearing_deg(angle)) {
+        return 0U;
+    }
+    // waypoint current nav index
+    uint32_t value = MIN(mission->get_current_nav_index(), WP_NUMBER_LIMIT);
+    // distance to next waypoint
+    value |= prep_number(wp_distance, 3, 2) << WP_DISTANCE_OFFSET;
+    // bearing encoded in 3 degrees increments
+    value |= ((uint8_t)roundf(wrap_360(angle) * 0.333f)) << WP_BEARING_OFFSET;
+    return value;
+}
 
 /*
   fetch Sport data for an external transport, such as FPort or crossfire

libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h

@@ -74,6 +74,7 @@ public:
 #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
         TERRAIN =       14, // 0x500B terrain data
         WIND =          15, // 0x500C wind data
+        WAYPOINT =      16, // 0x500D waypoint data
         WFQ_LAST_ITEM       // must be last
     };
 
@@ -110,6 +111,7 @@ private:
     uint32_t calc_rpm(void);
     uint32_t calc_terrain(void);
     uint32_t calc_wind(void);
+    uint32_t calc_waypoint(void);
 
     // use_external_data is set when this library will
     // be providing data to another transport, such as FPort