From 9ef4595ad348e0c646cff302edd5ada1277fd55b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 5 Jun 2012 11:18:45 +1000
Subject: [PATCH] autotest: fixed lat/lon update for multicopter sim

this makes waypoint tracking more accurate
---
 Tools/autotest/pysim/aircraft.py | 12 +++++-------
 Tools/autotest/pysim/util.py     | 18 ++++++++++++++++++
 Tools/autotest/sim_arducopter.sh |  3 ++-
 3 files changed, 25 insertions(+), 8 deletions(-)

diff --git a/Tools/autotest/pysim/aircraft.py b/Tools/autotest/pysim/aircraft.py
index 08aaaa8f40..02ef939f97 100644
--- a/Tools/autotest/pysim/aircraft.py
+++ b/Tools/autotest/pysim/aircraft.py
@@ -34,16 +34,14 @@ class Aircraft(object):
             position = self.position
         return (-position.z) + self.home_altitude <= self.ground_level + self.frame_height
 
-
     def update_position(self, delta_time):
         '''update lat/lon/alt from position'''
 
-        radius_of_earth = 6378100.0 # in meters
-        dlat = math.degrees(math.atan(self.position.x/radius_of_earth))
-        self.latitude  = self.home_latitude + dlat
-        lon_scale = math.cos(math.radians(self.latitude));
-        dlon = math.degrees(math.atan(self.position.y/radius_of_earth))/lon_scale
-        self.longitude = self.home_longitude + dlon
+        bearing = math.degrees(math.atan2(self.position.y, self.position.x))
+        distance = math.sqrt(self.position.x**2 + self.position.y**2)
+
+        (self.latitude, self.longitude) = util.gps_newpos(self.home_latitude, self.home_longitude,
+                                                          bearing, distance)
 
         self.altitude  = self.home_altitude - self.position.z
 
diff --git a/Tools/autotest/pysim/util.py b/Tools/autotest/pysim/util.py
index 11d44eac55..0ea6e58b74 100644
--- a/Tools/autotest/pysim/util.py
+++ b/Tools/autotest/pysim/util.py
@@ -252,6 +252,24 @@ def BodyRatesToEarthRates(dcm, gyro):
     psiDot   = (q*sin(phi) + r*cos(phi))/cos(theta)
     return Vector3(phiDot, thetaDot, psiDot)
 
+def gps_newpos(lat, lon, bearing, distance):
+    '''extrapolate latitude/longitude given a heading and distance 
+    thanks to http://www.movable-type.co.uk/scripts/latlong.html
+    '''
+    from math import sin, asin, cos, atan2, radians, degrees
+    radius_of_earth = 6378100.0 # in meters
+    
+    lat1 = radians(lat)
+    lon1 = radians(lon)
+    brng = radians(bearing)
+    dr = distance/radius_of_earth
+    
+    lat2 = asin(sin(lat1)*cos(dr) +
+                cos(lat1)*sin(dr)*cos(brng))
+    lon2 = lon1 + atan2(sin(brng)*sin(dr)*cos(lat1), 
+                        cos(dr)-sin(lat1)*sin(lat2))
+    return (degrees(lat2), degrees(lon2))
+
 
 class Wind(object):
     '''a wind generation object'''
diff --git a/Tools/autotest/sim_arducopter.sh b/Tools/autotest/sim_arducopter.sh
index 37e3421d38..93e540c863 100755
--- a/Tools/autotest/sim_arducopter.sh
+++ b/Tools/autotest/sim_arducopter.sh
@@ -28,7 +28,8 @@ make clean $target
 
 tfile=$(tempfile)
 echo r > $tfile
-gnome-terminal -e "gdb -x $tfile --args /tmp/ArduCopter.build/ArduCopter.elf"
+#gnome-terminal -e "gdb -x $tfile --args /tmp/ArduCopter.build/ArduCopter.elf"
+gnome-terminal -e /tmp/ArduCopter.build/ArduCopter.elf
 #gnome-terminal -e "valgrind -q /tmp/ArduCopter.build/ArduCopter.elf"
 sleep 2
 rm -f $tfile