2011-11-12 07:13:17 -04:00
# fly ArduPlane in SIL
import util , pexpect , sys , time , math , shutil , os
from common import *
2011-12-02 16:55:03 -04:00
import mavutil , random
2011-11-12 07:13:17 -04:00
# get location of scripts
testdir = os . path . dirname ( os . path . realpath ( __file__ ) )
HOME_LOCATION = ' -35.362938,149.165085,584,270 '
2011-12-18 03:57:20 -04:00
WIND = " 0,180,0.2 " # speed,direction,variance
2011-11-12 07:13:17 -04:00
homeloc = None
def takeoff ( mavproxy , mav ) :
''' takeoff get to 30m altitude '''
mavproxy . send ( ' switch 4 \n ' )
wait_mode ( mav , ' FBWA ' )
# some rudder to counteract the prop torque
2011-12-18 03:57:20 -04:00
mavproxy . send ( ' rc 4 1700 \n ' )
# some up elevator to keep the tail down
mavproxy . send ( ' rc 2 1200 \n ' )
2011-11-12 07:13:17 -04:00
2011-12-18 02:29:36 -04:00
# get it moving a bit first
2011-12-18 20:23:38 -04:00
mavproxy . send ( ' rc 3 1150 \n ' )
2011-12-18 02:29:36 -04:00
mav . recv_match ( condition = ' VFR_HUD.groundspeed>2 ' , blocking = True )
2011-11-12 07:13:17 -04:00
# a bit faster
2011-12-18 03:57:20 -04:00
mavproxy . send ( ' rc 3 1300 \n ' )
mav . recv_match ( condition = ' VFR_HUD.groundspeed>6 ' , blocking = True )
# a bit faster again, straighten rudder
2011-11-12 07:13:17 -04:00
mavproxy . send ( ' rc 3 1600 \n ' )
2011-12-18 03:57:20 -04:00
mavproxy . send ( ' rc 4 1500 \n ' )
2011-12-18 02:29:36 -04:00
mav . recv_match ( condition = ' VFR_HUD.groundspeed>12 ' , blocking = True )
2011-11-12 07:13:17 -04:00
2011-12-18 03:57:20 -04:00
# hit the gas harder now, and give it some more elevator
mavproxy . send ( ' rc 2 1100 \n ' )
2011-11-12 07:13:17 -04:00
mavproxy . send ( ' rc 3 1800 \n ' )
# gain a bit of altitude
2012-02-13 20:19:31 -04:00
if not wait_altitude ( mav , homeloc . alt + 30 , homeloc . alt + 60 , timeout = 30 ) :
return False
2011-11-12 07:13:17 -04:00
# level off
mavproxy . send ( ' rc 2 1500 \n ' )
print ( " TAKEOFF COMPLETE " )
return True
def fly_left_circuit ( mavproxy , mav ) :
''' fly a left circuit, 200m on a side '''
mavproxy . send ( ' switch 4 \n ' )
wait_mode ( mav , ' FBWA ' )
2011-12-02 00:21:15 -04:00
mavproxy . send ( ' rc 3 2000 \n ' )
2012-02-13 20:19:31 -04:00
if not wait_level_flight ( mavproxy , mav ) :
return False
2011-11-12 07:13:17 -04:00
print ( " Flying left circuit " )
# do 4 turns
for i in range ( 0 , 4 ) :
# hard left
print ( " Starting turn %u " % i )
mavproxy . send ( ' rc 1 1000 \n ' )
2012-02-13 20:19:31 -04:00
if not wait_heading ( mav , 270 - ( 90 * i ) ) :
return False
2011-11-12 07:13:17 -04:00
mavproxy . send ( ' rc 1 1500 \n ' )
print ( " Starting leg %u " % i )
2012-02-13 20:19:31 -04:00
if not wait_distance ( mav , 100 ) :
return False
2011-11-12 07:13:17 -04:00
print ( " Circuit complete " )
return True
def fly_RTL ( mavproxy , mav ) :
''' fly to home '''
2011-12-02 00:21:15 -04:00
print ( " Flying home in RTL " )
2011-11-12 07:13:17 -04:00
mavproxy . send ( ' switch 2 \n ' )
wait_mode ( mav , ' RTL ' )
2012-02-13 20:19:31 -04:00
if not wait_location ( mav , homeloc , accuracy = 90 ,
target_altitude = homeloc . alt + 100 , height_accuracy = 20 ,
timeout = 90 ) :
return False
2011-11-12 07:13:17 -04:00
print ( " RTL Complete " )
return True
2012-02-13 20:19:31 -04:00
def fly_LOITER ( mavproxy , mav , num_circles = 4 ) :
2011-11-12 07:13:17 -04:00
''' loiter where we are '''
2012-02-13 20:19:31 -04:00
print ( " Testing LOITER for %u turns " % num_circles )
2011-11-12 07:13:17 -04:00
mavproxy . send ( ' switch 3 \n ' )
wait_mode ( mav , ' LOITER ' )
2012-02-13 20:19:31 -04:00
while num_circles > 0 :
if not wait_heading ( mav , 0 ) :
return False
if not wait_heading ( mav , 180 ) :
return False
num_circles - = 1
print ( " Loiter %u circles left " % num_circles )
print ( " Completed Loiter OK " )
2011-11-12 07:13:17 -04:00
return True
2011-11-13 21:59:59 -04:00
def wait_level_flight ( mavproxy , mav , accuracy = 5 , timeout = 30 ) :
''' wait for level flight '''
tstart = time . time ( )
2012-02-13 20:19:31 -04:00
print ( " Waiting for level flight " )
2011-11-13 21:59:59 -04:00
while time . time ( ) < tstart + timeout :
m = mav . recv_match ( type = ' ATTITUDE ' , blocking = True )
roll = math . degrees ( m . roll )
pitch = math . degrees ( m . pitch )
print ( " Roll= %.1f Pitch= %.1f " % ( roll , pitch ) )
if math . fabs ( roll ) < = accuracy and math . fabs ( pitch ) < = accuracy :
2012-02-13 20:19:31 -04:00
print ( " Attained level flight " )
2011-11-13 21:59:59 -04:00
return True
print ( " Failed to attain level flight " )
return False
2011-11-12 07:13:17 -04:00
def change_altitude ( mavproxy , mav , altitude , accuracy = 10 ) :
''' get to a given altitude '''
mavproxy . send ( ' switch 4 \n ' )
wait_mode ( mav , ' FBWA ' )
alt_error = mav . messages [ ' VFR_HUD ' ] . alt - altitude
if alt_error > 0 :
mavproxy . send ( ' rc 2 2000 \n ' )
else :
mavproxy . send ( ' rc 2 1000 \n ' )
2012-02-13 20:19:31 -04:00
if not wait_altitude ( mav , altitude - accuracy / 2 , altitude + accuracy / 2 ) :
return False
2011-11-12 07:13:17 -04:00
mavproxy . send ( ' rc 2 1500 \n ' )
print ( " Reached target altitude at %u " % mav . messages [ ' VFR_HUD ' ] . alt )
2011-11-13 21:59:59 -04:00
return wait_level_flight ( mavproxy , mav )
2011-11-12 07:13:17 -04:00
def axial_left_roll ( mavproxy , mav , count = 1 ) :
''' fly a left axial roll '''
# full throttle!
mavproxy . send ( ' rc 3 2000 \n ' )
2012-02-13 20:19:31 -04:00
if not change_altitude ( mavproxy , mav , homeloc . alt + 200 ) :
return False
2011-11-12 07:13:17 -04:00
# fly the roll in manual
2011-11-13 21:59:59 -04:00
mavproxy . send ( ' switch 6 \n ' )
wait_mode ( mav , ' MANUAL ' )
2011-11-12 07:13:17 -04:00
while count > 0 :
print ( " Starting roll " )
mavproxy . send ( ' rc 1 1000 \n ' )
2012-02-13 20:19:31 -04:00
if not wait_roll ( mav , - 150 , accuracy = 20 ) :
return False
if not wait_roll ( mav , 150 , accuracy = 20 ) :
return False
if not wait_roll ( mav , 0 , accuracy = 20 ) :
return False
2011-11-12 07:13:17 -04:00
count - = 1
# back to FBWA
mavproxy . send ( ' rc 1 1500 \n ' )
mavproxy . send ( ' switch 4 \n ' )
wait_mode ( mav , ' FBWA ' )
mavproxy . send ( ' rc 3 1700 \n ' )
2011-11-13 21:59:59 -04:00
return wait_level_flight ( mavproxy , mav )
def inside_loop ( mavproxy , mav , count = 1 ) :
''' fly a inside loop '''
# full throttle!
mavproxy . send ( ' rc 3 2000 \n ' )
2012-02-13 20:19:31 -04:00
if not change_altitude ( mavproxy , mav , homeloc . alt + 200 ) :
return False
2011-11-13 21:59:59 -04:00
# fly the loop in manual
mavproxy . send ( ' switch 6 \n ' )
wait_mode ( mav , ' MANUAL ' )
while count > 0 :
print ( " Starting loop " )
mavproxy . send ( ' rc 2 1000 \n ' )
2012-02-13 20:19:31 -04:00
if not wait_pitch ( mav , 80 , accuracy = 20 ) :
return False
if not wait_pitch ( mav , 0 , accuracy = 20 ) :
return False
2011-11-13 21:59:59 -04:00
count - = 1
# back to FBWA
mavproxy . send ( ' rc 2 1500 \n ' )
mavproxy . send ( ' switch 4 \n ' )
wait_mode ( mav , ' FBWA ' )
mavproxy . send ( ' rc 3 1700 \n ' )
return wait_level_flight ( mavproxy , mav )
2011-11-12 07:13:17 -04:00
def setup_rc ( mavproxy ) :
''' setup RC override control '''
for chan in [ 1 , 2 , 4 , 5 , 6 , 7 ] :
mavproxy . send ( ' rc %u 1500 \n ' % chan )
mavproxy . send ( ' rc 3 1000 \n ' )
mavproxy . send ( ' rc 8 1800 \n ' )
def fly_mission ( mavproxy , mav , filename , height_accuracy = - 1 , target_altitude = None ) :
''' fly a mission from a file '''
global homeloc
2012-02-13 20:19:31 -04:00
print ( " Flying mission %s " % filename )
2011-11-12 07:13:17 -04:00
mavproxy . send ( ' wp load %s \n ' % filename )
mavproxy . expect ( ' flight plan received ' )
mavproxy . send ( ' wp list \n ' )
mavproxy . expect ( ' Requesting [0-9]+ waypoints ' )
mavproxy . send ( ' switch 1 \n ' ) # auto mode
wait_mode ( mav , ' AUTO ' )
2011-12-18 20:23:38 -04:00
if not wait_waypoint ( mav , 1 , 7 , max_dist = 60 ) :
2011-11-12 07:13:17 -04:00
return False
2011-12-18 20:23:38 -04:00
if not wait_groundspeed ( mav , 0 , 0.5 , timeout = 60 ) :
2011-11-12 07:13:17 -04:00
return False
print ( " Mission OK " )
return True
def fly_ArduPlane ( viewerip = None ) :
''' fly ArduPlane in SIL
you can pass viewerip as an IP address to optionally send fg and
mavproxy packets too for local viewing of the flight in real time
'''
2011-12-02 00:21:15 -04:00
global homeloc
2011-11-12 07:13:17 -04:00
2011-12-02 00:21:15 -04:00
options = ' --sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=5 '
2011-11-12 07:13:17 -04:00
if viewerip :
2011-12-02 00:21:15 -04:00
options + = " --out= %s :14550 " % viewerip
2011-11-13 21:20:55 -04:00
sil = util . start_SIL ( ' ArduPlane ' , wipe = True )
mavproxy = util . start_MAVProxy_SIL ( ' ArduPlane ' , options = options )
mavproxy . expect ( ' Received [0-9]+ parameters ' )
# setup test parameters
2011-12-02 16:55:03 -04:00
mavproxy . send ( ' param set SYSID_THISMAV %u \n ' % random . randint ( 100 , 200 ) )
2011-11-13 21:20:55 -04:00
mavproxy . send ( " param load %s /ArduPlane.parm \n " % testdir )
mavproxy . expect ( ' Loaded [0-9]+ parameters ' )
# restart with new parms
util . pexpect_close ( mavproxy )
util . pexpect_close ( sil )
2011-12-02 00:21:15 -04:00
cmd = util . reltopdir ( " Tools/autotest/jsbsim/runsim.py " )
2011-12-13 04:04:00 -04:00
cmd + = " --home= %s --wind= %s " % ( HOME_LOCATION , WIND )
2011-12-02 00:21:15 -04:00
if viewerip :
cmd + = " --fgout= %s :5503 " % viewerip
runsim = pexpect . spawn ( cmd , logfile = sys . stdout , timeout = 10 )
runsim . delaybeforesend = 0
util . pexpect_autoclose ( runsim )
runsim . expect ( ' Simulator ready to fly ' )
2011-11-13 21:20:55 -04:00
sil = util . start_SIL ( ' ArduPlane ' )
2011-11-12 07:13:17 -04:00
mavproxy = util . start_MAVProxy_SIL ( ' ArduPlane ' , options = options )
mavproxy . expect ( ' Logging to ( \ S+) ' )
logfile = mavproxy . match . group ( 1 )
print ( " LOGFILE %s " % logfile )
buildlog = util . reltopdir ( " ../buildlogs/ArduPlane-test.mavlog " )
print ( " buildlog= %s " % buildlog )
if os . path . exists ( buildlog ) :
os . unlink ( buildlog )
os . link ( logfile , buildlog )
mavproxy . expect ( ' Received [0-9]+ parameters ' )
util . expect_setup_callback ( mavproxy , expect_callback )
2011-12-02 00:21:15 -04:00
expect_list_clear ( )
expect_list_extend ( [ runsim , sil , mavproxy ] )
2011-11-12 07:13:17 -04:00
2011-12-02 00:21:15 -04:00
print ( " Started simulator " )
2011-11-12 07:13:17 -04:00
# get a mavlink connection going
try :
mav = mavutil . mavlink_connection ( ' 127.0.0.1:19550 ' , robust_parsing = True )
except Exception , msg :
print ( " Failed to start mavlink connection on 127.0.0.1:19550 " % msg )
raise
mav . message_hooks . append ( message_hook )
2011-11-27 23:13:38 -04:00
mav . idle_hooks . append ( idle_hook )
2011-11-12 07:13:17 -04:00
failed = False
e = ' None '
try :
mav . wait_heartbeat ( )
setup_rc ( mavproxy )
2011-12-02 00:21:15 -04:00
mav . recv_match ( type = ' GPS_RAW ' , condition = ' MAV.flightmode!= " INITIALISING " and GPS_RAW.fix_type==2 and GPS_RAW.lat != 0 and GPS_RAW.alt != 0 and VFR_HUD.alt > 10 ' ,
2011-11-12 07:13:17 -04:00
blocking = True )
homeloc = current_location ( mav )
print ( " Home location: %s " % homeloc )
if not takeoff ( mavproxy , mav ) :
2011-11-13 20:26:34 -04:00
print ( " Failed takeoff " )
2011-11-12 07:13:17 -04:00
failed = True
if not fly_left_circuit ( mavproxy , mav ) :
2011-11-13 20:26:34 -04:00
print ( " Failed left circuit " )
2011-11-12 07:13:17 -04:00
failed = True
if not axial_left_roll ( mavproxy , mav , 1 ) :
2011-11-13 20:26:34 -04:00
print ( " Failed left roll " )
2011-11-12 07:13:17 -04:00
failed = True
2011-11-13 21:59:59 -04:00
if not inside_loop ( mavproxy , mav ) :
print ( " Failed inside loop " )
failed = True
2011-11-12 07:13:17 -04:00
if not fly_RTL ( mavproxy , mav ) :
2011-11-13 20:26:34 -04:00
print ( " Failed RTL " )
2011-11-12 07:13:17 -04:00
failed = True
2012-02-13 20:19:31 -04:00
if not fly_LOITER ( mavproxy , mav ) :
print ( " Failed LOITER " )
failed = True
2011-11-12 07:13:17 -04:00
if not fly_mission ( mavproxy , mav , os . path . join ( testdir , " ap1.txt " ) , height_accuracy = 10 ,
target_altitude = homeloc . alt + 100 ) :
2011-11-13 20:26:34 -04:00
print ( " Failed mission " )
2011-11-12 07:13:17 -04:00
failed = True
except pexpect . TIMEOUT , e :
failed = True
2011-12-02 00:21:15 -04:00
mav . close ( )
2011-11-12 07:13:17 -04:00
util . pexpect_close ( mavproxy )
util . pexpect_close ( sil )
2011-12-02 00:21:15 -04:00
util . pexpect_close ( runsim )
2011-11-12 07:13:17 -04:00
if os . path . exists ( ' ArduPlane-valgrind.log ' ) :
os . chmod ( ' ArduPlane-valgrind.log ' , 0644 )
shutil . copy ( " ArduPlane-valgrind.log " , util . reltopdir ( " ../buildlogs/ArduPlane-valgrind.log " ) )
if failed :
print ( " FAILED: %s " % e )
return False
return True