From f76a7e64cacb3a9156a892f476db494d911449b0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 19 Jun 2016 20:57:34 +1000
Subject: [PATCH] SITL: auto-select correct data fields from X-Plane10

---
 libraries/SITL/SIM_XPlane.cpp | 66 ++++++++++++++++++++++++++++-------
 libraries/SITL/SIM_XPlane.h   |  1 +
 2 files changed, 55 insertions(+), 12 deletions(-)

diff --git a/libraries/SITL/SIM_XPlane.cpp b/libraries/SITL/SIM_XPlane.cpp
index 787a6c5b4f..59afad6f0f 100644
--- a/libraries/SITL/SIM_XPlane.cpp
+++ b/libraries/SITL/SIM_XPlane.cpp
@@ -44,6 +44,44 @@ XPlane::XPlane(const char *home_str, const char *frame_str) :
     socket_in.bind("0.0.0.0", bind_port);
 }
 
+/*
+ change what data is requested from XPlane  
+ */
+void XPlane::select_data(uint64_t usel_mask, uint64_t sel_mask)
+{
+    struct PACKED {
+        uint8_t  marker[5] { 'D', 'S', 'E', 'L', '0' };
+        uint32_t data[8] {};
+    } dsel;
+    uint8_t count = 0;
+    for (uint8_t i=0; i<64 && count<8; i++) {
+        if ((((uint64_t)1)<<i) & sel_mask) {
+            dsel.data[count++] = i;
+            printf("i=%u\n", (unsigned)i);
+        }
+    }
+    if (count != 0) {
+        socket_out.send(&dsel, sizeof(dsel));
+        printf("Selecting %u data types 0x%llx\n", (unsigned)count, (unsigned long long)sel_mask);
+    }
+
+    struct PACKED {
+        uint8_t  marker[5] { 'U', 'S', 'E', 'L', '0' };
+        uint32_t data[8] {};
+    } usel;
+    count = 0;
+    for (uint8_t i=0; i<64 && count<8; i++) {
+        if ((((uint64_t)1)<<i) & usel_mask) {
+            usel.data[count++] = i;
+            printf("i=%u\n", (unsigned)i);
+        }
+    }
+    if (count != 0) {
+        socket_out.send(&usel, sizeof(usel));
+        printf("De-selecting %u data types 0x%llx\n", (unsigned)count, (unsigned long long)usel_mask);
+    }
+}
+    
 /*
   receive data from X-Plane via UDP
 */
@@ -53,7 +91,9 @@ bool XPlane::receive_data(void)
     uint8_t *p = &pkt[5];
     const uint8_t pkt_len = 36;
     uint64_t data_mask = 0;
-    const uint64_t required_mask = (1U<<Times | 1U<<LatLonAlt | 1U<<PitchRollHeading | 1U<<LocVelDistTraveled | 1U<<AngularVelocities);
+    const uint64_t required_mask = (1U<<Times | 1U<<LatLonAlt | 1U<<Speed | 1U<<PitchRollHeading |
+                                    1U<<LocVelDistTraveled | 1U<<AngularVelocities | 1U<<Gload |
+                                    1U << Joystick1 | 1U << ThrottleCommand);
     Location loc {};
     Vector3f pos;
     ssize_t len = socket_in.recv(pkt, sizeof(pkt), 1);
@@ -64,12 +104,21 @@ bool XPlane::receive_data(void)
     }
     len -= 5;
 
+    if (!connected) {
+        // we now know the IP X-Plane is using
+        uint16_t port;
+        socket_in.last_recv_address(xplane_ip, port);
+        socket_out.connect(xplane_ip, xplane_port);
+        connected = true;
+        printf("Connected to %s:%u\n", xplane_ip, (unsigned)xplane_port);
+    }
+    
     while (len >= pkt_len) {
         const float *data = (const float *)p;
         uint8_t code = p[0];
         // keep a mask of what codes we have received
         if (code < 64) {
-            data_mask |= (1U << code);
+            data_mask |= (((uint64_t)1) << code);
         }
         switch (code) {
         case Times: {
@@ -165,8 +214,9 @@ bool XPlane::receive_data(void)
         p += pkt_len;
     }
 
-    if ((data_mask & required_mask) != required_mask) {
-        printf("Not receiving all required data, missing 0x%08lx\n", (unsigned long)(required_mask & ~data_mask));
+    if (data_mask != required_mask) {
+        // ask XPlane to change what data it sends
+        select_data(data_mask & ~required_mask, required_mask & ~data_mask);
         goto failed;
     }
     position = pos + position_zero;
@@ -232,14 +282,6 @@ failed:
 */
 void XPlane::send_data(const struct sitl_input &input)
 {
-    if (!connected) {
-        uint16_t port;
-        socket_in.last_recv_address(xplane_ip, port);
-        socket_out.connect(xplane_ip, xplane_port);
-        connected = true;
-        printf("Connected to %s:%u\n", xplane_ip, (unsigned)xplane_port);
-    }
-
     float aileron  = (input.servos[0]-1500)/500.0f;
     float elevator = (input.servos[1]-1500)/500.0f;
     float throttle = (input.servos[2]-1000)/1000.0;
diff --git a/libraries/SITL/SIM_XPlane.h b/libraries/SITL/SIM_XPlane.h
index a4c9368cab..9b13e67d18 100644
--- a/libraries/SITL/SIM_XPlane.h
+++ b/libraries/SITL/SIM_XPlane.h
@@ -43,6 +43,7 @@ public:
 private:
     bool receive_data(void);
     void send_data(const struct sitl_input &input);
+    void select_data(uint64_t usel_mask, uint64_t sel_mask);
 
     const char *xplane_ip = "127.0.0.1";
     uint16_t xplane_port = 49000;