From ad6c90c3b9b326ea50fcab646d7bd920814a146d Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 13 Dec 2011 15:28:03 +1100
Subject: [PATCH] autotest: added wind support to ArduPlane simulation

---
 Tools/autotest/jsbsim/runsim.py | 22 ++++++++++++++++++++--
 Tools/autotest/pysim/util.py    | 33 +++++++++++++++++++--------------
 Tools/autotest/sim_arduplane.sh |  2 +-
 3 files changed, 40 insertions(+), 17 deletions(-)

diff --git a/Tools/autotest/jsbsim/runsim.py b/Tools/autotest/jsbsim/runsim.py
index 22ccb9261a..9657dd7639 100755
--- a/Tools/autotest/jsbsim/runsim.py
+++ b/Tools/autotest/jsbsim/runsim.py
@@ -69,6 +69,13 @@ def process_sitl_input(buf):
         jsb_set('fcs/throttle-cmd-norm', throttle)
         sitl_state.throttle = throttle
 
+def update_wind(wind):
+    '''update wind simulation'''
+    (speed, direction) = wind.current()
+    jsb_set('atmosphere/psiw-rad', math.radians(direction))
+    jsb_set('atmosphere/wind-mag-fps', speed/0.3048)
+    
+
 def process_jsb_input(buf):
     '''process FG FDM input from JSBSim'''
     global fdm, fg_out, sim_out
@@ -119,6 +126,7 @@ parser.add_option("--fgout",   help="FG display output (IP:port)",   default="12
 parser.add_option("--home",    type='string', help="home lat,lng,alt,hdg (required)")
 parser.add_option("--script",  type='string', help='jsbsim model script', default='jsbsim/rascal_test.xml')
 parser.add_option("--options", type='string', help='jsbsim startup options')
+parser.add_option("--wind", dest="wind", help="Simulate wind (speed,direction,turbulance)", default='0,0,0')
 
 (opts, args) = parser.parse_args()
 
@@ -186,6 +194,10 @@ if opts.fgout:
     fg_out.connect(interpret_address(opts.fgout))
 
 
+# setup wind generator
+wind = util.Wind(opts.wind)
+
+
 setup_home(opts.home)
 
 fdm = fgFDM.fgFDM()
@@ -200,8 +212,10 @@ print("Simulator ready to fly")
 
 def main_loop():
     '''run main loop'''
-    last_report = time.time()
-    last_sim_input = time.time()
+    tnow = time.time()
+    last_report = tnow
+    last_sim_input = tnow
+    last_wind_update = tnow
     frame_count = 0
     paused = False
 
@@ -242,6 +256,10 @@ def main_loop():
                 paused = False
                 jsb_console.send('resume\n')
 
+        if tnow - last_wind_update > 0.1:
+            update_wind(wind)
+            last_wind_update = tnow
+
         if tnow - last_report > 0.5:
             print("FPS %u asl=%.1f agl=%.1f roll=%.1f pitch=%.1f a=(%.2f %.2f %.2f)" % (
                 frame_count / (time.time() - last_report),
diff --git a/Tools/autotest/pysim/util.py b/Tools/autotest/pysim/util.py
index 6e51d37f04..67e13c5ad9 100644
--- a/Tools/autotest/pysim/util.py
+++ b/Tools/autotest/pysim/util.py
@@ -346,7 +346,7 @@ class Wind(object):
         if len(a) != 3:
             raise RuntimeError("Expected wind in speed,direction,turbulance form, not %s" % windstring)
         self.speed     = float(a[0]) # m/s
-        self.direction = float(a[1]) # direction the wind is coming from
+        self.direction = float(a[1]) # direction the wind is going in
         self.turbulance= float(a[2]) # turbulance factor (standard deviation)
 
         # the cross-section of the aircraft to wind. This is multiplied by the
@@ -362,34 +362,39 @@ class Wind(object):
 
         # initial turbulance multiplier
         self.turbulance_mul = 1.0
-        
-
-    def accel(self, velocity, deltat=None):
-        '''return current wind acceleration in ground frame.  The
-           velocity is a Vector3 of the current velocity of the aircraft
-           in earth frame, m/s'''
 
+    def current(self, deltat=None):
+        '''return current wind speed and direction as a tuple
+        speed is in m/s, direction in degrees
+        '''
         if deltat is None:
             tnow = time.time()
             deltat = tnow - self.tlast
             self.tlast = tnow
 
-        # wind vector
-        v = euclid.Vector3(-self.speed, 0, 0)
-        wind = euclid.Quaternion.new_rotate_euler(0, math.radians(self.direction), 0) * v
-
         # update turbulance random walk
         w_delta = math.sqrt(deltat)*(1.0-random.gauss(1.0, self.turbulance))
         w_delta -= (self.turbulance_mul-1.0)*(deltat/self.turbulance_time_constant)
         self.turbulance_mul += w_delta
         
-        # add in turbulance
-        wind *= self.turbulance_mul
+        speed = self.speed * self.turbulance_mul
+        return (speed, self.direction)
+
+    def accel(self, velocity, deltat=None):
+        '''return current wind acceleration in ground frame.  The
+           velocity is a Vector3 of the current velocity of the aircraft
+           in earth frame, m/s'''
+
+        (speed, direction) = self.current(deltat=deltat)
+
+        # wind vector
+        v = euclid.Vector3(speed, 0, 0)
+        wind = euclid.Quaternion.new_rotate_euler(0, math.radians(direction), 0) * v
 
         # relative wind vector
         relwind = wind - velocity
 
-        # we ignore turbulance for now
+        # add in cross-section effect
         a = relwind * self.cross_section
 
         return a
diff --git a/Tools/autotest/sim_arduplane.sh b/Tools/autotest/sim_arduplane.sh
index e2fd0e9c09..45b6ce0965 100755
--- a/Tools/autotest/sim_arduplane.sh
+++ b/Tools/autotest/sim_arduplane.sh
@@ -12,6 +12,6 @@ echo r > $tfile
 gnome-terminal -e "gdb -x $tfile --args /tmp/ArduPlane.build/ArduPlane.elf"
 sleep 2
 rm -f $tfile
-gnome-terminal -e '../Tools/autotest/jsbsim/runsim.py --home=-35.362938,149.165085,584,270'
+gnome-terminal -e '../Tools/autotest/jsbsim/runsim.py --home=-35.362938,149.165085,584,270 --wind=5,180,0.2'
 sleep 2
 mavproxy.py --master tcp:127.0.0.1:5760 --sitl 127.0.0.1:5501 --out 127.0.0.1:14550 --out 127.0.0.1:14551