From 3b0cd9f101398a57a8dfec2f0b2a0758971be99c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 4 May 2016 12:49:42 +1000
Subject: [PATCH] SITL: added RC input from FlightAxis

use the interlink controller for input for 6 channels
---
 libraries/SITL/SIM_Aircraft.cpp   |  2 ++
 libraries/SITL/SIM_Aircraft.h     |  2 ++
 libraries/SITL/SIM_FlightAxis.cpp | 15 +++++++++++++--
 libraries/SITL/SIM_FlightAxis.h   |  9 +++++++++
 libraries/SITL/SITL.h             |  2 ++
 5 files changed, 28 insertions(+), 2 deletions(-)

diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index 41333236de..a907d057cc 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -296,6 +296,8 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) const
     fdm.battery_current = battery_current;
     fdm.rpm1 = rpm1;
     fdm.rpm2 = rpm2;
+    fdm.rcin_chan_count = rcin_chan_count;
+    memcpy(fdm.rcin, rcin, rcin_chan_count*sizeof(float));
 }
 
 uint64_t Aircraft::get_wall_time_us() const
diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h
index 60b0a5fe39..ec4e2fcf46 100644
--- a/libraries/SITL/SIM_Aircraft.h
+++ b/libraries/SITL/SIM_Aircraft.h
@@ -119,6 +119,8 @@ protected:
     float battery_current = 0;
     float rpm1 = 0;
     float rpm2 = 0;
+    uint8_t rcin_chan_count = 0;
+    float rcin[8];
 
     uint64_t time_now_us;
 
diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp
index 87832710f0..24ef1d3615 100644
--- a/libraries/SITL/SIM_FlightAxis.cpp
+++ b/libraries/SITL/SIM_FlightAxis.cpp
@@ -66,8 +66,11 @@ void FlightAxis::parse_reply(const char *reply)
         }
         p += strlen(keytable[i].key) + 1;
         keytable[i].ref = atof(p);
-        // this assumes key order
-        reply = p;
+        // this assumes key order and allows us to decode arrays
+        p = strchr(p, '>');
+        if (p != nullptr) {
+            reply = p;
+        }
     }
 }
 
@@ -296,6 +299,14 @@ void FlightAxis::update(const struct sitl_input &input)
     battery_current = state.m_batteryCurrentDraw_AMPS;
     rpm1 = state.m_propRPM;
     rpm2 = state.m_heliMainRotorRPM;
+
+    /*
+      the interlink interface doesn't seem to support more than 6 channels
+     */
+    for (uint8_t i=0; i<6; i++) {
+        rcin[i] = state.rcin[i];
+    }
+    rcin_chan_count = 6;
     
     update_position();
     time_now_us = (state.m_currentPhysicsTime_SEC - initial_time_s)*1.0e6;
diff --git a/libraries/SITL/SIM_FlightAxis.h b/libraries/SITL/SIM_FlightAxis.h
index 719f0133a4..8f978d1265 100644
--- a/libraries/SITL/SIM_FlightAxis.h
+++ b/libraries/SITL/SIM_FlightAxis.h
@@ -41,6 +41,7 @@ public:
     }
 
     struct state {
+        double rcin[8];
         double m_airspeed_MPS;
         double m_altitudeASL_MTR;
         double m_altitudeAGL_MTR;
@@ -94,6 +95,14 @@ public:
         const char *key;
         double &ref;
     } keytable[num_keys] = {
+        { "item", state.rcin[0] },
+        { "item", state.rcin[1] },
+        { "item", state.rcin[2] },
+        { "item", state.rcin[3] },
+        { "item", state.rcin[4] },
+        { "item", state.rcin[5] },
+        { "item", state.rcin[6] },
+        { "item", state.rcin[7] },
         { "m-airspeed-MPS", state.m_airspeed_MPS },
         { "m-altitudeASL-MTR", state.m_altitudeASL_MTR },
         { "m-altitudeAGL-MTR", state.m_altitudeAGL_MTR },
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index 73c3c495d9..e7fce576c8 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -23,6 +23,8 @@ struct sitl_fdm {
     double battery_current; // Amps
     double rpm1;            // main prop RPM
     double rpm2;            // secondary RPM
+    uint8_t rcin_chan_count;
+    float  rcin[8];         // RC input 0..1
 };
 
 // number of rc output channels