From 871219d1996f831bf385b5b9d6a0c466fd74c5f2 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 12 Jul 2016 14:39:44 +1000
Subject: [PATCH] SITL: support helicopters with X-Plane 10

we get collective from "throttle2" and get interlock from generator1 on/off
---
 libraries/SITL/SIM_XPlane.cpp | 105 +++++++++++++++++++++++++---------
 libraries/SITL/SIM_XPlane.h   |   7 +++
 2 files changed, 85 insertions(+), 27 deletions(-)

diff --git a/libraries/SITL/SIM_XPlane.cpp b/libraries/SITL/SIM_XPlane.cpp
index c638f8168d..25db18664f 100644
--- a/libraries/SITL/SIM_XPlane.cpp
+++ b/libraries/SITL/SIM_XPlane.cpp
@@ -41,6 +41,9 @@ XPlane::XPlane(const char *home_str, const char *frame_str) :
     if (colon) {
         xplane_ip = colon+1;
     }
+
+    heli_frame = (strstr(frame_str, "-heli") != nullptr);
+
     socket_in.bind("0.0.0.0", bind_port);
     printf("Waiting for XPlane data on UDP port %u and sending to port %u\n",
            (unsigned)bind_port, (unsigned)xplane_port);
@@ -98,9 +101,11 @@ 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<<Speed | 1U<<PitchRollHeading |
-                                    1U<<LocVelDistTraveled | 1U<<AngularVelocities | 1U<<Gload |
-                                    1U << Joystick1 | 1U << ThrottleCommand | 1U << Trim);
+    const uint64_t one = 1U;
+    const uint64_t required_mask = (one<<Times | one<<LatLonAlt | one<<Speed | one<<PitchRollHeading |
+                                    one<<LocVelDistTraveled | one<<AngularVelocities | one<<Gload |
+                                    one << Joystick1 | one << ThrottleCommand | one << Trim |
+                                    one << PropPitch | one << EngineRPM | one << PropRPM | one << Generator);
     Location loc {};
     Vector3f pos;
     uint32_t wait_time_ms = 1;
@@ -213,28 +218,50 @@ bool XPlane::receive_data(void)
             break;
 
         case ThrottleCommand: {
-            /* getting joystick throttle input is very weird. The
-             * problem is that XPlane sends the ThrottleCommand packet
-             * both for joystick throttle input and for throttle that
-             * we have provided over the link. So we need some way to
-             * detect when we get our own values back. The trick used
-             * is to add throttle_magic to the values we send, then
-             * detect this offset in the data coming back. Very ugly,
-             * but I can't find a better way of allowing joystick
-             * input from XPlane10
-             */
-            bool has_magic = ((uint32_t)(data[1] * throttle_magic_scale) % 1000U) == (uint32_t)(throttle_magic * throttle_magic_scale);
-            if (data[1] < 0 ||
-                data[1] == throttle_sent ||
-                has_magic) {
-                break;
+            if (heli_frame) {
+                // on helis we setup the joystick to use "throttle2" for collective input
+                rcin[2] = data[2];
+            } else {
+                /* getting joystick throttle input is very weird. The
+                 * problem is that XPlane sends the ThrottleCommand packet
+                 * both for joystick throttle input and for throttle that
+                 * we have provided over the link. So we need some way to
+                 * detect when we get our own values back. The trick used
+                 * is to add throttle_magic to the values we send, then
+                 * detect this offset in the data coming back. Very ugly,
+                 * but I can't find a better way of allowing joystick
+                 * input from XPlane10
+                 */
+                bool has_magic = ((uint32_t)(data[1] * throttle_magic_scale) % 1000U) == (uint32_t)(throttle_magic * throttle_magic_scale);
+                if (data[1] < 0 ||
+                    data[1] == throttle_sent ||
+                    has_magic) {
+                    break;
+                }
+                rcin[2] = data[1];
             }
-            rcin[2] = data[1];
             break;
         }
+
+        case PropPitch: {
+            break;
+        }
+
+        case EngineRPM:
+            rpm1 = data[1];
+            break;
+
+        case PropRPM:
+            rpm2 = data[1];
+            break;
             
         case Joystick2:
             break;
+
+        case Generator:
+            rcin_chan_count = 8;
+            rcin[7] = data[1];
+            break;
             
         }
         len -= pkt_len;
@@ -354,20 +381,44 @@ void XPlane::send_data(const struct sitl_input &input)
         }
     }
 
-    d.code = 11;
+    d.code = FlightCon;
     d.data[0] = elevator;
     d.data[1] = aileron;
     d.data[2] = rudder;
     d.data[4] = rudder;
     socket_out.send(&d, sizeof(d));
 
-    d.code = 25;
-    d.data[0] = throttle;
-    d.data[1] = throttle;
-    d.data[2] = throttle;
-    d.data[3] = throttle;
-    d.data[4] = 0;
-    socket_out.send(&d, sizeof(d));
+    if (!heli_frame) {
+        d.code = ThrottleCommand;
+        d.data[0] = throttle;
+        d.data[1] = throttle;
+        d.data[2] = throttle;
+        d.data[3] = throttle;
+        d.data[4] = 0;
+        socket_out.send(&d, sizeof(d));
+    } else {
+        // send chan3 as collective pitch, on scale from -10 to +10
+        float collective = 10*(input.servos[2]-1500)/500.0;
+
+        // and send throttle from channel 8
+        throttle = (input.servos[7]-1000)/1000.0;
+
+        d.code = PropPitch;
+        d.data[0] = collective;
+        d.data[1] = -rudder*10; // reverse sense of rudder, 10 degrees pitch range
+        d.data[2] = 0;
+        d.data[3] = 0;
+        d.data[4] = 0;
+        socket_out.send(&d, sizeof(d));
+
+        d.code = ThrottleCommand;
+        d.data[0] = throttle;
+        d.data[1] = throttle;
+        d.data[2] = throttle;
+        d.data[3] = throttle;
+        d.data[4] = 0;
+        socket_out.send(&d, sizeof(d));
+    }
 
     throttle_sent = throttle;
 }
diff --git a/libraries/SITL/SIM_XPlane.h b/libraries/SITL/SIM_XPlane.h
index 9b20b0ef2e..48b74a70b1 100644
--- a/libraries/SITL/SIM_XPlane.h
+++ b/libraries/SITL/SIM_XPlane.h
@@ -67,6 +67,9 @@ private:
     } report;
     float last_flap;
 
+    // are we controlling a heli?
+    bool heli_frame;
+    
     // throttle joystick input is very weird. See comments in the main code
     const float throttle_magic = 0.000123f;
     const float throttle_magic_scale = 1.0e6;
@@ -97,6 +100,10 @@ private:
         LatLonAlt           = 20,
 		LocVelDistTraveled  = 21,
         ThrottleCommand     = 25,
+        EngineRPM           = 37,
+        PropRPM             = 38,
+        PropPitch           = 39,
+        Generator           = 58,
 	};
 };