From 091a1e7fe0b86cb60aafb8e8ed17005b075a1ac7 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 23 May 2015 11:03:57 +1000
Subject: [PATCH] SITL: fixed wind support in JSBSim

---
 libraries/SITL/SIM_Aircraft.h | 5 +++++
 libraries/SITL/SIM_JSBSim.cpp | 9 ++++++---
 2 files changed, 11 insertions(+), 3 deletions(-)

diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h
index c1dc6ee5d5..508688cd20 100644
--- a/libraries/SITL/SIM_Aircraft.h
+++ b/libraries/SITL/SIM_Aircraft.h
@@ -38,6 +38,11 @@ public:
      */
     struct sitl_input {
         uint16_t servos[16];
+        struct {
+            float speed;      // m/s
+            float direction;  // degrees 0..360
+            float turbulance;
+        } wind;
     };
 
     /*
diff --git a/libraries/SITL/SIM_JSBSim.cpp b/libraries/SITL/SIM_JSBSim.cpp
index 071a38df7c..5022f160ff 100644
--- a/libraries/SITL/SIM_JSBSim.cpp
+++ b/libraries/SITL/SIM_JSBSim.cpp
@@ -34,6 +34,7 @@ extern const AP_HAL::HAL& hal;
 #pragma GCC diagnostic ignored "-Wunused-result"
 
 #define DEBUG_JSBSIM 1
+#define FEET_TO_METERS 0.3048f
 
 /*
   constructor
@@ -336,8 +337,12 @@ void JSBSim::send_servos(const struct sitl_input &input)
              "set fcs/elevator-cmd-norm %f\n"
              "set fcs/rudder-cmd-norm %f\n"
              "set fcs/throttle-cmd-norm %f\n"
+             "set atmosphere/psiw-rad %f\n"
+             "set atmosphere/wind-mag-fps %f\n"
              "step\n",
-             aileron, elevator, rudder, throttle);
+             aileron, elevator, rudder, throttle,
+             radians(input.wind.direction),
+             input.wind.speed / FEET_TO_METERS);
     ssize_t buflen = strlen(buf);
     ssize_t sent = sock_control.send(buf, buflen);
     free(buf);
@@ -353,8 +358,6 @@ void JSBSim::send_servos(const struct sitl_input &input)
     }
 }
 
-#define FEET_TO_METERS 0.3048f
-
 /* nasty hack ....
    JSBSim sends in little-endian
  */