autotest: fixed lat/lon update for multicopter sim

this makes waypoint tracking more accurate
This commit is contained in:
Andrew Tridgell 2012-06-05 11:18:45 +10:00
parent 3431064d09
commit 9ef4595ad3
3 changed files with 25 additions and 8 deletions

View File

@ -34,16 +34,14 @@ class Aircraft(object):
position = self.position position = self.position
return (-position.z) + self.home_altitude <= self.ground_level + self.frame_height return (-position.z) + self.home_altitude <= self.ground_level + self.frame_height
def update_position(self, delta_time): def update_position(self, delta_time):
'''update lat/lon/alt from position''' '''update lat/lon/alt from position'''
radius_of_earth = 6378100.0 # in meters bearing = math.degrees(math.atan2(self.position.y, self.position.x))
dlat = math.degrees(math.atan(self.position.x/radius_of_earth)) distance = math.sqrt(self.position.x**2 + self.position.y**2)
self.latitude = self.home_latitude + dlat
lon_scale = math.cos(math.radians(self.latitude)); (self.latitude, self.longitude) = util.gps_newpos(self.home_latitude, self.home_longitude,
dlon = math.degrees(math.atan(self.position.y/radius_of_earth))/lon_scale bearing, distance)
self.longitude = self.home_longitude + dlon
self.altitude = self.home_altitude - self.position.z self.altitude = self.home_altitude - self.position.z

View File

@ -252,6 +252,24 @@ def BodyRatesToEarthRates(dcm, gyro):
psiDot = (q*sin(phi) + r*cos(phi))/cos(theta) psiDot = (q*sin(phi) + r*cos(phi))/cos(theta)
return Vector3(phiDot, thetaDot, psiDot) 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): class Wind(object):
'''a wind generation object''' '''a wind generation object'''

View File

@ -28,7 +28,8 @@ make clean $target
tfile=$(tempfile) tfile=$(tempfile)
echo r > $tfile 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" #gnome-terminal -e "valgrind -q /tmp/ArduCopter.build/ArduCopter.elf"
sleep 2 sleep 2
rm -f $tfile rm -f $tfile