mirror of https://github.com/ArduPilot/ardupilot
1474 lines
48 KiB
Python
1474 lines
48 KiB
Python
#!/usr/bin/env python
|
|
|
|
# Fly ArduCopter in SITL
|
|
from __future__ import print_function
|
|
import math
|
|
import os
|
|
import shutil
|
|
import time
|
|
|
|
import pexpect
|
|
from pymavlink import mavutil, mavwp
|
|
|
|
from common import *
|
|
from pysim import util
|
|
from pysim import vehicleinfo
|
|
|
|
vinfo = vehicleinfo.VehicleInfo()
|
|
|
|
# get location of scripts
|
|
testdir = os.path.dirname(os.path.realpath(__file__))
|
|
|
|
#################################################
|
|
# CONFIG
|
|
#################################################
|
|
HOME = mavutil.location(-35.362938, 149.165085, 584, 270)
|
|
AVCHOME = mavutil.location(40.072842, -105.230575, 1586, 0)
|
|
|
|
homeloc = None
|
|
num_wp = 0
|
|
|
|
# Flight mode switch positions are set-up in arducopter.param to be
|
|
# switch 1 = Circle
|
|
# switch 2 = Land
|
|
# switch 3 = RTL
|
|
# switch 4 = Auto
|
|
# switch 5 = Loiter
|
|
# switch 6 = Stabilize
|
|
|
|
|
|
#################################################
|
|
# UTILITIES
|
|
#################################################
|
|
def takeoff(mavproxy, mav, alt_min=30, takeoff_throttle=1700):
|
|
"""Takeoff get to 30m altitude."""
|
|
mavproxy.send('switch 6\n') # stabilize mode
|
|
wait_mode(mav, 'STABILIZE')
|
|
set_rc(mavproxy, mav, 3, takeoff_throttle)
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
if (m.alt < alt_min):
|
|
wait_altitude(mav, alt_min, (alt_min + 5))
|
|
hover(mavproxy, mav)
|
|
progress("TAKEOFF COMPLETE")
|
|
return True
|
|
|
|
|
|
def land(mavproxy, mav, timeout=60):
|
|
"""Land the quad."""
|
|
progress("STARTING LANDING")
|
|
mavproxy.send('switch 2\n') # land mode
|
|
wait_mode(mav, 'LAND')
|
|
progress("Entered Landing Mode")
|
|
ret = wait_altitude(mav, -5, 1)
|
|
progress("LANDING: ok= %s" % ret)
|
|
return ret
|
|
|
|
|
|
def hover(mavproxy, mav, hover_throttle=1500):
|
|
set_rc(mavproxy, mav, 3, hover_throttle)
|
|
return True
|
|
|
|
|
|
# loiter - fly south west, then hold loiter within 5m position and altitude
|
|
def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5):
|
|
"""Hold loiter position."""
|
|
mavproxy.send('switch 5\n') # loiter mode
|
|
wait_mode(mav, 'LOITER')
|
|
|
|
# first aim south east
|
|
progress("turn south east")
|
|
set_rc(mavproxy, mav, 4, 1580)
|
|
if not wait_heading(mav, 170):
|
|
return False
|
|
set_rc(mavproxy, mav, 4, 1500)
|
|
|
|
# fly south east 50m
|
|
set_rc(mavproxy, mav, 2, 1100)
|
|
if not wait_distance(mav, 50):
|
|
return False
|
|
set_rc(mavproxy, mav, 2, 1500)
|
|
|
|
# wait for copter to slow moving
|
|
if not wait_groundspeed(mav, 0, 2):
|
|
return False
|
|
|
|
success = True
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
start_altitude = m.alt
|
|
start = mav.location()
|
|
tstart = get_sim_time(mav)
|
|
tholdstart = get_sim_time(mav)
|
|
progress("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime))
|
|
while get_sim_time(mav) < tstart + holdtime:
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
pos = mav.location()
|
|
delta = get_distance(start, pos)
|
|
alt_delta = math.fabs(m.alt - start_altitude)
|
|
progress("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt))
|
|
if alt_delta > maxaltchange:
|
|
progress("Loiter alt shifted %u meters (> limit of %u)" % (alt_delta, maxaltchange))
|
|
success = False
|
|
if delta > maxdistchange:
|
|
progress("Loiter shifted %u meters (> limit of %u)" % (delta, maxdistchange))
|
|
success = False
|
|
if success:
|
|
progress("Loiter OK for %u seconds" % holdtime)
|
|
else:
|
|
progress("Loiter FAILED")
|
|
return success
|
|
|
|
|
|
def change_alt(mavproxy, mav, alt_min, climb_throttle=1920, descend_throttle=1080):
|
|
"""Change altitude."""
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
if(m.alt < alt_min):
|
|
progress("Rise to alt:%u from %u" % (alt_min, m.alt))
|
|
set_rc(mavproxy, mav, 3, climb_throttle)
|
|
wait_altitude(mav, alt_min, (alt_min + 5))
|
|
else:
|
|
progress("Lower to alt:%u from %u" % (alt_min, m.alt))
|
|
set_rc(mavproxy, mav, 3, descend_throttle)
|
|
wait_altitude(mav, (alt_min - 5), alt_min)
|
|
hover(mavproxy, mav)
|
|
return True
|
|
|
|
|
|
#################################################
|
|
# TESTS FLY
|
|
#################################################
|
|
# fly a square in stabilize mode
|
|
def fly_square(mavproxy, mav, side=50, timeout=300):
|
|
"""Fly a square, flying N then E ."""
|
|
tstart = get_sim_time(mav)
|
|
success = True
|
|
|
|
# ensure all sticks in the middle
|
|
set_rc(mavproxy, mav, 1, 1500)
|
|
set_rc(mavproxy, mav, 2, 1500)
|
|
set_rc(mavproxy, mav, 3, 1500)
|
|
set_rc(mavproxy, mav, 4, 1500)
|
|
|
|
# switch to loiter mode temporarily to stop us from rising
|
|
mavproxy.send('switch 5\n')
|
|
wait_mode(mav, 'LOITER')
|
|
|
|
# first aim north
|
|
progress("turn right towards north")
|
|
set_rc(mavproxy, mav, 4, 1580)
|
|
if not wait_heading(mav, 10):
|
|
progress("Failed to reach heading")
|
|
success = False
|
|
set_rc(mavproxy, mav, 4, 1500)
|
|
mav.recv_match(condition='RC_CHANNELS.chan4_raw==1500', blocking=True)
|
|
|
|
# save bottom left corner of box as waypoint
|
|
progress("Save WP 1 & 2")
|
|
save_wp(mavproxy, mav)
|
|
|
|
# switch back to stabilize mode
|
|
set_rc(mavproxy, mav, 3, 1500)
|
|
mavproxy.send('switch 6\n')
|
|
wait_mode(mav, 'STABILIZE')
|
|
|
|
# pitch forward to fly north
|
|
progress("Going north %u meters" % side)
|
|
set_rc(mavproxy, mav, 2, 1300)
|
|
if not wait_distance(mav, side):
|
|
progress("Failed to reach distance of %u" % side)
|
|
success = False
|
|
set_rc(mavproxy, mav, 2, 1500)
|
|
|
|
# save top left corner of square as waypoint
|
|
progress("Save WP 3")
|
|
save_wp(mavproxy, mav)
|
|
|
|
# roll right to fly east
|
|
progress("Going east %u meters" % side)
|
|
set_rc(mavproxy, mav, 1, 1700)
|
|
if not wait_distance(mav, side):
|
|
progress("Failed to reach distance of %u" % side)
|
|
success = False
|
|
set_rc(mavproxy, mav, 1, 1500)
|
|
|
|
# save top right corner of square as waypoint
|
|
progress("Save WP 4")
|
|
save_wp(mavproxy, mav)
|
|
|
|
# pitch back to fly south
|
|
progress("Going south %u meters" % side)
|
|
set_rc(mavproxy, mav, 2, 1700)
|
|
if not wait_distance(mav, side):
|
|
progress("Failed to reach distance of %u" % side)
|
|
success = False
|
|
set_rc(mavproxy, mav, 2, 1500)
|
|
|
|
# save bottom right corner of square as waypoint
|
|
progress("Save WP 5")
|
|
save_wp(mavproxy, mav)
|
|
|
|
# roll left to fly west
|
|
progress("Going west %u meters" % side)
|
|
set_rc(mavproxy, mav, 1, 1300)
|
|
if not wait_distance(mav, side):
|
|
progress("Failed to reach distance of %u" % side)
|
|
success = False
|
|
set_rc(mavproxy, mav, 1, 1500)
|
|
|
|
# save bottom left corner of square (should be near home) as waypoint
|
|
progress("Save WP 6")
|
|
save_wp(mavproxy, mav)
|
|
|
|
# descend to 10m
|
|
progress("Descend to 10m in Loiter")
|
|
mavproxy.send('switch 5\n') # loiter mode
|
|
wait_mode(mav, 'LOITER')
|
|
set_rc(mavproxy, mav, 3, 1300)
|
|
time_left = timeout - (get_sim_time(mav) - tstart)
|
|
progress("timeleft = %u" % time_left)
|
|
if time_left < 20:
|
|
time_left = 20
|
|
if not wait_altitude(mav, -10, 10, time_left):
|
|
progress("Failed to reach alt of 10m")
|
|
success = False
|
|
save_wp(mavproxy, mav)
|
|
|
|
return success
|
|
|
|
|
|
def fly_RTL(mavproxy, mav, side=60, timeout=250):
|
|
"""Return, land."""
|
|
progress("# Enter RTL")
|
|
mavproxy.send('switch 3\n')
|
|
tstart = get_sim_time(mav)
|
|
while get_sim_time(mav) < tstart + timeout:
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
pos = mav.location()
|
|
home_distance = get_distance(HOME, pos)
|
|
progress("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance))
|
|
if(m.alt <= 1 and home_distance < 10):
|
|
return True
|
|
return False
|
|
|
|
|
|
def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
|
|
"""Fly east, Failsafe, return, land."""
|
|
|
|
# switch to loiter mode temporarily to stop us from rising
|
|
mavproxy.send('switch 5\n')
|
|
wait_mode(mav, 'LOITER')
|
|
|
|
# first aim east
|
|
progress("turn east")
|
|
set_rc(mavproxy, mav, 4, 1580)
|
|
if not wait_heading(mav, 135):
|
|
return False
|
|
set_rc(mavproxy, mav, 4, 1500)
|
|
|
|
# raise throttle slightly to avoid hitting the ground
|
|
set_rc(mavproxy, mav, 3, 1600)
|
|
|
|
# switch to stabilize mode
|
|
mavproxy.send('switch 6\n')
|
|
wait_mode(mav, 'STABILIZE')
|
|
hover(mavproxy, mav)
|
|
|
|
# fly east 60 meters
|
|
progress("# Going forward %u meters" % side)
|
|
set_rc(mavproxy, mav, 2, 1350)
|
|
if not wait_distance(mav, side, 5, 60):
|
|
return False
|
|
set_rc(mavproxy, mav, 2, 1500)
|
|
|
|
# pull throttle low
|
|
progress("# Enter Failsafe")
|
|
set_rc(mavproxy, mav, 3, 900)
|
|
|
|
tstart = get_sim_time(mav)
|
|
while get_sim_time(mav) < tstart + timeout:
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
pos = mav.location()
|
|
home_distance = get_distance(HOME, pos)
|
|
progress("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance))
|
|
# check if we've reached home
|
|
if m.alt <= 1 and home_distance < 10:
|
|
# reduce throttle
|
|
set_rc(mavproxy, mav, 3, 1100)
|
|
# switch back to stabilize
|
|
mavproxy.send('switch 2\n') # land mode
|
|
wait_mode(mav, 'LAND')
|
|
progress("Waiting for disarm")
|
|
mav.motors_disarmed_wait()
|
|
progress("Reached failsafe home OK")
|
|
mavproxy.send('switch 6\n') # stabilize mode
|
|
wait_mode(mav, 'STABILIZE')
|
|
set_rc(mavproxy, mav, 3, 1000)
|
|
if not arm_vehicle(mavproxy, mav):
|
|
progress("Failed to re-arm")
|
|
return False
|
|
return True
|
|
progress("Failed to land on failsafe RTL - timed out after %u seconds" % timeout)
|
|
# reduce throttle
|
|
set_rc(mavproxy, mav, 3, 1100)
|
|
# switch back to stabilize mode
|
|
mavproxy.send('switch 2\n') # land mode
|
|
wait_mode(mav, 'LAND')
|
|
mavproxy.send('switch 6\n') # stabilize mode
|
|
wait_mode(mav, 'STABILIZE')
|
|
return False
|
|
|
|
|
|
def fly_battery_failsafe(mavproxy, mav, timeout=30):
|
|
# assume failure
|
|
success = False
|
|
|
|
# switch to loiter mode so that we hold position
|
|
mavproxy.send('switch 5\n')
|
|
wait_mode(mav, 'LOITER')
|
|
mavproxy.send("rc 3 1500\n")
|
|
|
|
# enable battery failsafe
|
|
mavproxy.send("param set FS_BATT_ENABLE 1\n")
|
|
|
|
# trigger low voltage
|
|
mavproxy.send('param set SIM_BATT_VOLTAGE 10\n')
|
|
|
|
# wait for LAND mode
|
|
new_mode = wait_mode(mav, 'LAND')
|
|
if new_mode == 'LAND':
|
|
success = True
|
|
|
|
# disable battery failsafe
|
|
mavproxy.send('param set FS_BATT_ENABLE 0\n')
|
|
|
|
# return status
|
|
if success:
|
|
progress("Successfully entered LAND mode after battery failsafe")
|
|
else:
|
|
progress("Failed to enter LAND mode after battery failsafe")
|
|
|
|
return success
|
|
|
|
|
|
# fly_stability_patch - fly south, then hold loiter within 5m position and altitude and reduce 1 motor to 60% efficiency
|
|
def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchange=10):
|
|
"""Hold loiter position."""
|
|
mavproxy.send('switch 5\n') # loiter mode
|
|
wait_mode(mav, 'LOITER')
|
|
|
|
# first south
|
|
progress("turn south")
|
|
set_rc(mavproxy, mav, 4, 1580)
|
|
if not wait_heading(mav, 180):
|
|
return False
|
|
set_rc(mavproxy, mav, 4, 1500)
|
|
|
|
# fly west 80m
|
|
set_rc(mavproxy, mav, 2, 1100)
|
|
if not wait_distance(mav, 80):
|
|
return False
|
|
set_rc(mavproxy, mav, 2, 1500)
|
|
|
|
# wait for copter to slow moving
|
|
if not wait_groundspeed(mav, 0, 2):
|
|
return False
|
|
|
|
success = True
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
start_altitude = m.alt
|
|
start = mav.location()
|
|
tstart = get_sim_time(mav)
|
|
tholdstart = get_sim_time(mav)
|
|
progress("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime))
|
|
|
|
# cut motor 1 to 55% efficiency
|
|
progress("Cutting motor 1 to 60% efficiency")
|
|
mavproxy.send('param set SIM_ENGINE_MUL 0.60\n')
|
|
|
|
while get_sim_time(mav) < tstart + holdtime:
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
pos = mav.location()
|
|
delta = get_distance(start, pos)
|
|
alt_delta = math.fabs(m.alt - start_altitude)
|
|
progress("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt))
|
|
if alt_delta > maxaltchange:
|
|
progress("Loiter alt shifted %u meters (> limit of %u)" % (alt_delta, maxaltchange))
|
|
success = False
|
|
if delta > maxdistchange:
|
|
progress("Loiter shifted %u meters (> limit of %u)" % (delta, maxdistchange))
|
|
success = False
|
|
|
|
# restore motor 1 to 100% efficiency
|
|
mavproxy.send('param set SIM_ENGINE_MUL 1.0\n')
|
|
|
|
if success:
|
|
progress("Stability patch and Loiter OK for %u seconds" % holdtime)
|
|
else:
|
|
progress("Stability Patch FAILED")
|
|
|
|
return success
|
|
|
|
|
|
# fly_fence_test - fly east until you hit the horizontal circular fence
|
|
def fly_fence_test(mavproxy, mav, timeout=180):
|
|
"""Hold loiter position."""
|
|
mavproxy.send('switch 5\n') # loiter mode
|
|
wait_mode(mav, 'LOITER')
|
|
|
|
# enable fence, disable avoidance
|
|
mavproxy.send('param set FENCE_ENABLE 1\n')
|
|
mavproxy.send('param set AVOID_ENABLE 0\n')
|
|
|
|
# first east
|
|
progress("turn east")
|
|
set_rc(mavproxy, mav, 4, 1580)
|
|
if not wait_heading(mav, 160):
|
|
return False
|
|
set_rc(mavproxy, mav, 4, 1500)
|
|
|
|
# fly forward (east) at least 20m
|
|
pitching_forward = True
|
|
set_rc(mavproxy, mav, 2, 1100)
|
|
if not wait_distance(mav, 20):
|
|
return False
|
|
|
|
# start timer
|
|
tstart = get_sim_time(mav)
|
|
while get_sim_time(mav) < tstart + timeout:
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
pos = mav.location()
|
|
home_distance = get_distance(HOME, pos)
|
|
progress("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance))
|
|
# recenter pitch sticks once we reach home so we don't fly off again
|
|
if pitching_forward and home_distance < 10:
|
|
pitching_forward = False
|
|
set_rc(mavproxy, mav, 2, 1500)
|
|
# disable fence
|
|
mavproxy.send('param set FENCE_ENABLE 0\n')
|
|
if m.alt <= 1 and home_distance < 10:
|
|
# reduce throttle
|
|
set_rc(mavproxy, mav, 3, 1000)
|
|
# switch mode to stabilize
|
|
mavproxy.send('switch 2\n') # land mode
|
|
wait_mode(mav, 'LAND')
|
|
progress("Waiting for disarm")
|
|
mav.motors_disarmed_wait()
|
|
progress("Reached home OK")
|
|
mavproxy.send('switch 6\n') # stabilize mode
|
|
wait_mode(mav, 'STABILIZE')
|
|
set_rc(mavproxy, mav, 3, 1000)
|
|
mavproxy.send('arm uncheck all\n') # remove if we ever clear battery failsafe flag on disarm
|
|
if not arm_vehicle(mavproxy, mav):
|
|
progress("Failed to re-arm")
|
|
mavproxy.send('arm check all\n') # remove if we ever clear battery failsafe flag on disarm
|
|
return False
|
|
mavproxy.send('arm check all\n') # remove if we ever clear battery failsafe flag on disarm
|
|
progress("Reached home OK")
|
|
return True
|
|
|
|
# disable fence, enable avoidance
|
|
mavproxy.send('param set FENCE_ENABLE 0\n')
|
|
mavproxy.send('param set AVOID_ENABLE 1\n')
|
|
|
|
# reduce throttle
|
|
set_rc(mavproxy, mav, 3, 1000)
|
|
# switch mode to stabilize
|
|
mavproxy.send('switch 2\n') # land mode
|
|
wait_mode(mav, 'LAND')
|
|
mavproxy.send('switch 6\n') # stabilize mode
|
|
wait_mode(mav, 'STABILIZE')
|
|
progress("Fence test failed to reach home - timed out after %u seconds" % timeout)
|
|
return False
|
|
|
|
|
|
# fly_alt_fence_test - fly up until you hit the fence
|
|
def fly_alt_max_fence_test(mavproxy, mav, timeout=180):
|
|
"""Hold loiter position."""
|
|
mavproxy.send('switch 5\n') # loiter mode
|
|
wait_mode(mav, 'LOITER')
|
|
|
|
# enable fence, disable avoidance
|
|
set_parameter(mavproxy, 'FENCE_ENABLE', 1)
|
|
set_parameter(mavproxy, 'AVOID_ENABLE', 0)
|
|
set_parameter(mavproxy, 'FENCE_TYPE', 1)
|
|
|
|
if not change_alt(mavproxy, mav, 10):
|
|
failed_test_msg = "change_alt climb failed"
|
|
progress(failed_test_msg)
|
|
return False
|
|
|
|
# first east
|
|
progress("turn east")
|
|
set_rc(mavproxy, mav, 4, 1580)
|
|
if not wait_heading(mav, 160):
|
|
return False
|
|
set_rc(mavproxy, mav, 4, 1500)
|
|
|
|
# fly forward (east) at least 20m
|
|
set_rc(mavproxy, mav, 2, 1100)
|
|
if not wait_distance(mav, 20):
|
|
return False
|
|
|
|
# stop flying forward and start flying up:
|
|
set_rc(mavproxy, mav, 2, 1500)
|
|
set_rc(mavproxy, mav, 3, 1800)
|
|
|
|
# wait for fence to trigger
|
|
wait_mode(mav, 'RTL')
|
|
|
|
progress("Waiting for disarm")
|
|
mav.motors_disarmed_wait()
|
|
|
|
set_rc(mavproxy, mav, 3, 1000)
|
|
|
|
mavproxy.send('switch 6\n') # stabilize mode
|
|
wait_mode(mav, 'STABILIZE')
|
|
mavproxy.send('arm uncheck all\n') # remove if we ever clear battery failsafe flag on disarm
|
|
if not arm_vehicle(mavproxy, mav):
|
|
progress("Failed to re-arm")
|
|
mavproxy.send('arm check all\n') # remove if we ever clear battery failsafe flag on disarm
|
|
return False
|
|
mavproxy.send('arm check all\n') # remove if we ever clear battery failsafe flag on disarm
|
|
|
|
return True
|
|
|
|
def fly_gps_glitch_loiter_test(mavproxy, mav, use_map=False, timeout=30, max_distance=20):
|
|
"""fly_gps_glitch_loiter_test.
|
|
|
|
Fly south east in loiter and test reaction to gps glitch."""
|
|
mavproxy.send('switch 5\n') # loiter mode
|
|
wait_mode(mav, 'LOITER')
|
|
|
|
# turn on simulator display of gps and actual position
|
|
if (use_map):
|
|
show_gps_and_sim_positions(mavproxy, True)
|
|
|
|
# set-up gps glitch array
|
|
glitch_lat = [0.0002996, 0.0006958, 0.0009431, 0.0009991, 0.0009444, 0.0007716, 0.0006221]
|
|
glitch_lon = [0.0000717, 0.0000912, 0.0002761, 0.0002626, 0.0002807, 0.0002049, 0.0001304]
|
|
glitch_num = len(glitch_lat)
|
|
progress("GPS Glitches:")
|
|
for i in range(1, glitch_num):
|
|
progress("glitch %d %.7f %.7f" % (i, glitch_lat[i], glitch_lon[i]))
|
|
|
|
# turn south east
|
|
progress("turn south east")
|
|
set_rc(mavproxy, mav, 4, 1580)
|
|
if not wait_heading(mav, 150):
|
|
if (use_map):
|
|
show_gps_and_sim_positions(mavproxy, False)
|
|
return False
|
|
set_rc(mavproxy, mav, 4, 1500)
|
|
|
|
# fly forward (south east) at least 60m
|
|
set_rc(mavproxy, mav, 2, 1100)
|
|
if not wait_distance(mav, 60):
|
|
if (use_map):
|
|
show_gps_and_sim_positions(mavproxy, False)
|
|
return False
|
|
set_rc(mavproxy, mav, 2, 1500)
|
|
|
|
# wait for copter to slow down
|
|
if not wait_groundspeed(mav, 0, 1):
|
|
if (use_map):
|
|
show_gps_and_sim_positions(mavproxy, False)
|
|
return False
|
|
|
|
# record time and position
|
|
tstart = get_sim_time(mav)
|
|
tnow = tstart
|
|
start_pos = sim_location(mav)
|
|
success = True
|
|
|
|
# initialise current glitch
|
|
glitch_current = 0
|
|
progress("Apply first glitch")
|
|
mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current])
|
|
mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
|
|
|
|
# record position for 30 seconds
|
|
while tnow < tstart + timeout:
|
|
tnow = get_sim_time(mav)
|
|
desired_glitch_num = int((tnow - tstart) * 2.2)
|
|
if desired_glitch_num > glitch_current and glitch_current != -1:
|
|
glitch_current = desired_glitch_num
|
|
# turn off glitching if we've reached the end of the glitch list
|
|
if glitch_current >= glitch_num:
|
|
glitch_current = -1
|
|
progress("Completed Glitches")
|
|
mavproxy.send('param set SIM_GPS_GLITCH_X 0\n')
|
|
mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n')
|
|
else:
|
|
progress("Applying glitch %u" % glitch_current)
|
|
# move onto the next glitch
|
|
mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current])
|
|
mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
|
|
|
|
# start displaying distance moved after all glitches applied
|
|
if (glitch_current == -1):
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
curr_pos = sim_location(mav)
|
|
moved_distance = get_distance(curr_pos, start_pos)
|
|
progress("Alt: %u Moved: %.0f" % (m.alt, moved_distance))
|
|
if moved_distance > max_distance:
|
|
progress("Moved over %u meters, Failed!" % max_distance)
|
|
success = False
|
|
|
|
# disable gps glitch
|
|
if glitch_current != -1:
|
|
glitch_current = -1
|
|
mavproxy.send('param set SIM_GPS_GLITCH_X 0\n')
|
|
mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n')
|
|
if (use_map):
|
|
show_gps_and_sim_positions(mavproxy, False)
|
|
|
|
if success:
|
|
progress("GPS glitch test passed! stayed within %u meters for %u seconds" % (max_distance, timeout))
|
|
else:
|
|
progress("GPS glitch test FAILED!")
|
|
return success
|
|
|
|
|
|
# fly_gps_glitch_auto_test - fly mission and test reaction to gps glitch
|
|
def fly_gps_glitch_auto_test(mavproxy, mav, use_map=False, timeout=120):
|
|
|
|
# set-up gps glitch array
|
|
glitch_lat = [0.0002996, 0.0006958, 0.0009431, 0.0009991, 0.0009444, 0.0007716, 0.0006221]
|
|
glitch_lon = [0.0000717, 0.0000912, 0.0002761, 0.0002626, 0.0002807, 0.0002049, 0.0001304]
|
|
glitch_num = len(glitch_lat)
|
|
progress("GPS Glitches:")
|
|
for i in range(1, glitch_num):
|
|
progress("glitch %d %.7f %.7f" % (i, glitch_lat[i], glitch_lon[i]))
|
|
|
|
# Fly mission #1
|
|
progress("# Load copter_glitch_mission")
|
|
# load the waypoint count
|
|
global homeloc
|
|
global num_wp
|
|
num_wp = load_mission_from_file(mavproxy, os.path.join(testdir, "copter_glitch_mission.txt"))
|
|
if not num_wp:
|
|
progress("load copter_glitch_mission failed")
|
|
return False
|
|
|
|
# turn on simulator display of gps and actual position
|
|
if (use_map):
|
|
show_gps_and_sim_positions(mavproxy, True)
|
|
|
|
progress("test: Fly a mission from 1 to %u" % num_wp)
|
|
mavproxy.send('wp set 1\n')
|
|
|
|
# switch into AUTO mode and raise throttle
|
|
mavproxy.send('switch 4\n') # auto mode
|
|
wait_mode(mav, 'AUTO')
|
|
set_rc(mavproxy, mav, 3, 1500)
|
|
|
|
# wait until 100m from home
|
|
if not wait_distance(mav, 100, 5, 60):
|
|
if (use_map):
|
|
show_gps_and_sim_positions(mavproxy, False)
|
|
return False
|
|
|
|
# record time and position
|
|
tstart = get_sim_time(mav)
|
|
tnow = tstart
|
|
|
|
# initialise current glitch
|
|
glitch_current = 0
|
|
progress("Apply first glitch")
|
|
mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current])
|
|
mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
|
|
|
|
# record position for 30 seconds
|
|
while glitch_current < glitch_num:
|
|
tnow = get_sim_time(mav)
|
|
desired_glitch_num = int((tnow - tstart) * 2.2)
|
|
if desired_glitch_num > glitch_current and glitch_current != -1:
|
|
glitch_current = desired_glitch_num
|
|
# apply next glitch
|
|
if glitch_current < glitch_num:
|
|
progress("Applying glitch %u" % glitch_current)
|
|
mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current])
|
|
mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
|
|
|
|
# turn off glitching
|
|
progress("Completed Glitches")
|
|
mavproxy.send('param set SIM_GPS_GLITCH_X 0\n')
|
|
mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n')
|
|
|
|
# continue with the mission
|
|
ret = wait_waypoint(mav, 0, num_wp-1, timeout=500)
|
|
|
|
# wait for arrival back home
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
pos = mav.location()
|
|
dist_to_home = get_distance(HOME, pos)
|
|
while dist_to_home > 5:
|
|
if get_sim_time(mav) > (tstart + timeout):
|
|
progress("GPS Glitch testing failed - exceeded timeout %u seconds" % timeout)
|
|
ret = False
|
|
break
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
pos = mav.location()
|
|
dist_to_home = get_distance(HOME, pos)
|
|
progress("Dist from home: %u" % dist_to_home)
|
|
|
|
# turn off simulator display of gps and actual position
|
|
if (use_map):
|
|
show_gps_and_sim_positions(mavproxy, False)
|
|
|
|
progress("GPS Glitch test Auto completed: passed=%s" % ret)
|
|
|
|
return ret
|
|
|
|
|
|
# fly_simple - assumes the simple bearing is initialised to be directly north
|
|
# flies a box with 100m west, 15 seconds north, 50 seconds east, 15 seconds south
|
|
def fly_simple(mavproxy, mav, side=50, timeout=120):
|
|
|
|
failed = False
|
|
|
|
# hold position in loiter
|
|
mavproxy.send('switch 5\n') # loiter mode
|
|
wait_mode(mav, 'LOITER')
|
|
|
|
# set SIMPLE mode for all flight modes
|
|
mavproxy.send('param set SIMPLE 63\n')
|
|
|
|
# switch to stabilize mode
|
|
mavproxy.send('switch 6\n')
|
|
wait_mode(mav, 'STABILIZE')
|
|
set_rc(mavproxy, mav, 3, 1500)
|
|
|
|
# fly south 50m
|
|
progress("# Flying south %u meters" % side)
|
|
set_rc(mavproxy, mav, 1, 1300)
|
|
if not wait_distance(mav, side, 5, 60):
|
|
failed = True
|
|
set_rc(mavproxy, mav, 1, 1500)
|
|
|
|
# fly west 8 seconds
|
|
progress("# Flying west for 8 seconds")
|
|
set_rc(mavproxy, mav, 2, 1300)
|
|
tstart = get_sim_time(mav)
|
|
while get_sim_time(mav) < (tstart + 8):
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
delta = (get_sim_time(mav) - tstart)
|
|
# progress("%u" % delta)
|
|
set_rc(mavproxy, mav, 2, 1500)
|
|
|
|
# fly north 25 meters
|
|
progress("# Flying north %u meters" % (side/2.0))
|
|
set_rc(mavproxy, mav, 1, 1700)
|
|
if not wait_distance(mav, side/2, 5, 60):
|
|
failed = True
|
|
set_rc(mavproxy, mav, 1, 1500)
|
|
|
|
# fly east 8 seconds
|
|
progress("# Flying east for 8 seconds")
|
|
set_rc(mavproxy, mav, 2, 1700)
|
|
tstart = get_sim_time(mav)
|
|
while get_sim_time(mav) < (tstart + 8):
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
delta = (get_sim_time(mav) - tstart)
|
|
# progress("%u" % delta)
|
|
set_rc(mavproxy, mav, 2, 1500)
|
|
|
|
# restore to default
|
|
mavproxy.send('param set SIMPLE 0\n')
|
|
|
|
# hover in place
|
|
hover(mavproxy, mav)
|
|
return not failed
|
|
|
|
|
|
# fly_super_simple - flies a circle around home for 45 seconds
|
|
def fly_super_simple(mavproxy, mav, timeout=45):
|
|
|
|
failed = False
|
|
|
|
# hold position in loiter
|
|
mavproxy.send('switch 5\n') # loiter mode
|
|
wait_mode(mav, 'LOITER')
|
|
|
|
# fly forward 20m
|
|
progress("# Flying forward 20 meters")
|
|
set_rc(mavproxy, mav, 2, 1300)
|
|
if not wait_distance(mav, 20, 5, 60):
|
|
failed = True
|
|
set_rc(mavproxy, mav, 2, 1500)
|
|
|
|
# set SUPER SIMPLE mode for all flight modes
|
|
mavproxy.send('param set SUPER_SIMPLE 63\n')
|
|
|
|
# switch to stabilize mode
|
|
mavproxy.send('switch 6\n')
|
|
wait_mode(mav, 'STABILIZE')
|
|
set_rc(mavproxy, mav, 3, 1500)
|
|
|
|
# start copter yawing slowly
|
|
set_rc(mavproxy, mav, 4, 1550)
|
|
|
|
# roll left for timeout seconds
|
|
progress("# rolling left from pilot's point of view for %u seconds" % timeout)
|
|
set_rc(mavproxy, mav, 1, 1300)
|
|
tstart = get_sim_time(mav)
|
|
while get_sim_time(mav) < (tstart + timeout):
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
delta = (get_sim_time(mav) - tstart)
|
|
|
|
# stop rolling and yawing
|
|
set_rc(mavproxy, mav, 1, 1500)
|
|
set_rc(mavproxy, mav, 4, 1500)
|
|
|
|
# restore simple mode parameters to default
|
|
mavproxy.send('param set SUPER_SIMPLE 0\n')
|
|
|
|
# hover in place
|
|
hover(mavproxy, mav)
|
|
return not failed
|
|
|
|
|
|
# fly_circle - flies a circle with 20m radius
|
|
def fly_circle(mavproxy, mav, maxaltchange=10, holdtime=36):
|
|
|
|
# hold position in loiter
|
|
mavproxy.send('switch 5\n') # loiter mode
|
|
wait_mode(mav, 'LOITER')
|
|
|
|
# face west
|
|
progress("turn west")
|
|
set_rc(mavproxy, mav, 4, 1580)
|
|
if not wait_heading(mav, 270):
|
|
return False
|
|
set_rc(mavproxy, mav, 4, 1500)
|
|
|
|
# set CIRCLE radius
|
|
mavproxy.send('param set CIRCLE_RADIUS 3000\n')
|
|
|
|
# fly forward (east) at least 100m
|
|
set_rc(mavproxy, mav, 2, 1100)
|
|
if not wait_distance(mav, 100):
|
|
return False
|
|
|
|
# return pitch stick back to middle
|
|
set_rc(mavproxy, mav, 2, 1500)
|
|
|
|
# set CIRCLE mode
|
|
mavproxy.send('switch 1\n') # circle mode
|
|
wait_mode(mav, 'CIRCLE')
|
|
|
|
# wait
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
start_altitude = m.alt
|
|
tstart = get_sim_time(mav)
|
|
tholdstart = get_sim_time(mav)
|
|
progress("Circle at %u meters for %u seconds" % (start_altitude, holdtime))
|
|
while get_sim_time(mav) < tstart + holdtime:
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
progress("heading %u" % m.heading)
|
|
|
|
progress("CIRCLE OK for %u seconds" % holdtime)
|
|
return True
|
|
|
|
|
|
# fly_auto_test - fly mission which tests a significant number of commands
|
|
def fly_auto_test(mavproxy, mav):
|
|
|
|
# Fly mission #1
|
|
progress("# Load copter_mission")
|
|
# load the waypoint count
|
|
global homeloc
|
|
global num_wp
|
|
num_wp = load_mission_from_file(mavproxy, os.path.join(testdir, "copter_mission.txt"))
|
|
if not num_wp:
|
|
progress("load copter_mission failed")
|
|
return False
|
|
|
|
progress("test: Fly a mission from 1 to %u" % num_wp)
|
|
mavproxy.send('wp set 1\n')
|
|
|
|
# switch into AUTO mode and raise throttle
|
|
mavproxy.send('switch 4\n') # auto mode
|
|
wait_mode(mav, 'AUTO')
|
|
set_rc(mavproxy, mav, 3, 1500)
|
|
|
|
# fly the mission
|
|
ret = wait_waypoint(mav, 0, num_wp-1, timeout=500)
|
|
|
|
# land if mission failed
|
|
if ret is False:
|
|
land(mavproxy, mav)
|
|
|
|
# set throttle to minimum
|
|
set_rc(mavproxy, mav, 3, 1000)
|
|
|
|
# wait for disarm
|
|
mav.motors_disarmed_wait()
|
|
progress("MOTORS DISARMED OK")
|
|
|
|
progress("Auto mission completed: passed=%s" % ret)
|
|
|
|
return ret
|
|
|
|
|
|
# fly_avc_test - fly AVC mission
|
|
def fly_avc_test(mavproxy, mav):
|
|
|
|
# upload mission from file
|
|
progress("# Load copter_AVC2013_mission")
|
|
# load the waypoint count
|
|
global homeloc
|
|
global num_wp
|
|
num_wp = load_mission_from_file(mavproxy, os.path.join(testdir, "copter_AVC2013_mission.txt"))
|
|
if not num_wp:
|
|
progress("load copter_AVC2013_mission failed")
|
|
return False
|
|
|
|
progress("Fly AVC mission from 1 to %u" % num_wp)
|
|
mavproxy.send('wp set 1\n')
|
|
|
|
# wait for motor runup
|
|
wait_seconds(mav, 20)
|
|
|
|
# switch into AUTO mode and raise throttle
|
|
mavproxy.send('switch 4\n') # auto mode
|
|
wait_mode(mav, 'AUTO')
|
|
set_rc(mavproxy, mav, 3, 1500)
|
|
|
|
# fly the mission
|
|
ret = wait_waypoint(mav, 0, num_wp-1, timeout=500)
|
|
|
|
# set throttle to minimum
|
|
set_rc(mavproxy, mav, 3, 1000)
|
|
|
|
# wait for disarm
|
|
mav.motors_disarmed_wait()
|
|
progress("MOTORS DISARMED OK")
|
|
|
|
progress("AVC mission completed: passed=%s" % ret)
|
|
|
|
return ret
|
|
|
|
|
|
def fly_mission(mavproxy, mav, height_accuracy=-1.0, target_altitude=None):
|
|
"""Fly a mission from a file."""
|
|
global homeloc
|
|
global num_wp
|
|
progress("test: Fly a mission from 1 to %u" % num_wp)
|
|
mavproxy.send('wp set 1\n')
|
|
mavproxy.send('switch 4\n') # auto mode
|
|
wait_mode(mav, 'AUTO')
|
|
ret = wait_waypoint(mav, 0, num_wp-1, timeout=500)
|
|
progress("test: MISSION COMPLETE: passed=%s" % ret)
|
|
# wait here until ready
|
|
mavproxy.send('switch 5\n') # loiter mode
|
|
wait_mode(mav, 'LOITER')
|
|
return ret
|
|
|
|
|
|
def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=False, frame=None, params=None, gdbserver=False, speedup=10):
|
|
"""Fly ArduCopter in SITL.
|
|
|
|
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
|
|
"""
|
|
global homeloc
|
|
|
|
if frame is None:
|
|
frame = '+'
|
|
|
|
home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading)
|
|
sitl = util.start_SITL(binary, wipe=True, model=frame, home=home, speedup=speedup)
|
|
mavproxy = util.start_MAVProxy_SITL('ArduCopter')
|
|
mavproxy.expect('Received [0-9]+ parameters')
|
|
|
|
# setup test parameters
|
|
if params is None:
|
|
params = vinfo.options["ArduCopter"]["frames"][frame]["default_params_filename"]
|
|
if not isinstance(params, list):
|
|
params = [params]
|
|
for x in params:
|
|
mavproxy.send("param load %s\n" % os.path.join(testdir, x))
|
|
mavproxy.expect('Loaded [0-9]+ parameters')
|
|
mavproxy.send("param set LOG_REPLAY 1\n")
|
|
mavproxy.send("param set LOG_DISARMED 1\n")
|
|
time.sleep(3)
|
|
|
|
# reboot with new parameters
|
|
util.pexpect_close(mavproxy)
|
|
util.pexpect_close(sitl)
|
|
|
|
sitl = util.start_SITL(binary, model=frame, home=home, speedup=speedup, valgrind=valgrind, gdb=gdb, gdbserver=gdbserver)
|
|
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5'
|
|
if viewerip:
|
|
options += ' --out=%s:14550' % viewerip
|
|
if use_map:
|
|
options += ' --map'
|
|
mavproxy = util.start_MAVProxy_SITL('ArduCopter', options=options)
|
|
mavproxy.expect('Telemetry log: (\S+)\r\n')
|
|
logfile = mavproxy.match.group(1)
|
|
progress("LOGFILE %s" % logfile)
|
|
|
|
buildlog = util.reltopdir("../buildlogs/ArduCopter-test.tlog")
|
|
progress("buildlog=%s" % buildlog)
|
|
copy_tlog = False
|
|
if os.path.exists(buildlog):
|
|
os.unlink(buildlog)
|
|
try:
|
|
os.link(logfile, buildlog)
|
|
except Exception:
|
|
progress("WARN: Failed to create symlink: " + logfile + " => " + buildlog + ", Will copy tlog manually to target location")
|
|
copy_tlog = True
|
|
|
|
# the received parameters can come before or after the ready to fly message
|
|
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
|
|
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
|
|
|
|
util.expect_setup_callback(mavproxy, expect_callback)
|
|
|
|
expect_list_clear()
|
|
expect_list_extend([sitl, mavproxy])
|
|
|
|
# get a mavlink connection going
|
|
try:
|
|
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
|
|
except Exception as msg:
|
|
progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
|
|
raise
|
|
mav.message_hooks.append(message_hook)
|
|
mav.idle_hooks.append(idle_hook)
|
|
|
|
failed = False
|
|
failed_test_msg = "None"
|
|
|
|
try:
|
|
progress("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION)
|
|
mav.wait_heartbeat()
|
|
progress("Setting up RC parameters")
|
|
set_rc_default(mavproxy)
|
|
set_rc(mavproxy, mav, 3, 1000)
|
|
homeloc = mav.location()
|
|
|
|
progress("Home location: %s" % homeloc)
|
|
mavproxy.send('switch 6\n') # stabilize mode
|
|
mav.wait_heartbeat()
|
|
wait_mode(mav, 'STABILIZE')
|
|
progress("Waiting reading for arm")
|
|
wait_ready_to_arm(mav)
|
|
|
|
# Arm
|
|
progress("# Arm motors")
|
|
if not arm_vehicle(mavproxy, mav):
|
|
failed_test_msg = "arm_motors failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
progress("# Takeoff")
|
|
if not takeoff(mavproxy, mav, 10):
|
|
failed_test_msg = "takeoff failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# Fly a square in Stabilize mode
|
|
progress("#")
|
|
progress("########## Fly a square and save WPs with CH7 switch ##########")
|
|
progress("#")
|
|
if not fly_square(mavproxy, mav):
|
|
failed_test_msg = "fly_square failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# save the stored mission to file
|
|
progress("# Save out the CH7 mission to file")
|
|
global num_wp
|
|
num_wp = save_mission_to_file(mavproxy, os.path.join(testdir, "ch7_mission.txt"))
|
|
if not num_wp:
|
|
failed_test_msg = "save_mission_to_file failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# fly the stored mission
|
|
progress("# Fly CH7 saved mission")
|
|
if not fly_mission(mavproxy, mav, height_accuracy=0.5, target_altitude=10):
|
|
failed_test_msg = "fly ch7_mission failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# Throttle Failsafe
|
|
progress("#")
|
|
progress("########## Test Failsafe ##########")
|
|
progress("#")
|
|
if not fly_throttle_failsafe(mavproxy, mav):
|
|
failed_test_msg = "fly_throttle_failsafe failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# Takeoff
|
|
progress("# Takeoff")
|
|
if not takeoff(mavproxy, mav, 10):
|
|
failed_test_msg = "takeoff failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# Battery failsafe
|
|
if not fly_battery_failsafe(mavproxy, mav):
|
|
failed_test_msg = "fly_battery_failsafe failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# Takeoff
|
|
progress("# Takeoff")
|
|
if not takeoff(mavproxy, mav, 10):
|
|
failed_test_msg = "takeoff failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# Stability patch
|
|
progress("#")
|
|
progress("########## Test Stability Patch ##########")
|
|
progress("#")
|
|
if not fly_stability_patch(mavproxy, mav, 30):
|
|
failed_test_msg = "fly_stability_patch failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# RTL
|
|
progress("# RTL #")
|
|
if not fly_RTL(mavproxy, mav):
|
|
failed_test_msg = "fly_RTL after stab patch failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# Takeoff
|
|
progress("# Takeoff")
|
|
if not takeoff(mavproxy, mav, 10):
|
|
failed_test_msg = "takeoff failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# Fence test
|
|
progress("#")
|
|
progress("########## Test Horizontal Fence ##########")
|
|
progress("#")
|
|
if not fly_fence_test(mavproxy, mav, 180):
|
|
failed_test_msg = "fly_fence_test failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# Fence test
|
|
progress("#")
|
|
progress("########## Test Max Alt Fence ##########")
|
|
progress("#")
|
|
if not fly_alt_max_fence_test(mavproxy, mav, 180):
|
|
failed_test_msg = "fly_alt_max_fence_test failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# Takeoff
|
|
progress("# Takeoff")
|
|
if not takeoff(mavproxy, mav, 10):
|
|
failed_test_msg = "takeoff failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# Fly GPS Glitch Loiter test
|
|
progress("# GPS Glitch Loiter Test")
|
|
if not fly_gps_glitch_loiter_test(mavproxy, mav, use_map):
|
|
failed_test_msg = "fly_gps_glitch_loiter_test failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# RTL after GPS Glitch Loiter test
|
|
progress("# RTL #")
|
|
if not fly_RTL(mavproxy, mav):
|
|
failed_test_msg = "fly_RTL failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# Fly GPS Glitch test in auto mode
|
|
progress("# GPS Glitch Auto Test")
|
|
if not fly_gps_glitch_auto_test(mavproxy, mav, use_map):
|
|
failed_test_msg = "fly_gps_glitch_auto_test failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# take-off ahead of next test
|
|
progress("# Takeoff")
|
|
if not takeoff(mavproxy, mav, 10):
|
|
failed_test_msg = "takeoff failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# Loiter for 10 seconds
|
|
progress("#")
|
|
progress("########## Test Loiter for 10 seconds ##########")
|
|
progress("#")
|
|
if not loiter(mavproxy, mav):
|
|
failed_test_msg = "loiter failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# Loiter Climb
|
|
progress("#")
|
|
progress("# Loiter - climb to 30m")
|
|
progress("#")
|
|
if not change_alt(mavproxy, mav, 30):
|
|
failed_test_msg = "change_alt climb failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# Loiter Descend
|
|
progress("#")
|
|
progress("# Loiter - descend to 20m")
|
|
progress("#")
|
|
if not change_alt(mavproxy, mav, 20):
|
|
failed_test_msg = "change_alt descend failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# RTL
|
|
progress("#")
|
|
progress("########## Test RTL ##########")
|
|
progress("#")
|
|
if not fly_RTL(mavproxy, mav):
|
|
failed_test_msg = "fly_RTL after Loiter climb/descend failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# Takeoff
|
|
progress("# Takeoff")
|
|
if not takeoff(mavproxy, mav, 10):
|
|
failed_test_msg = "takeoff failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# Simple mode
|
|
progress("# Fly in SIMPLE mode")
|
|
if not fly_simple(mavproxy, mav):
|
|
failed_test_msg = "fly_simple failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# RTL
|
|
progress("#")
|
|
progress("########## Test RTL ##########")
|
|
progress("#")
|
|
if not fly_RTL(mavproxy, mav):
|
|
failed_test_msg = "fly_RTL after simple mode failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# Takeoff
|
|
progress("# Takeoff")
|
|
if not takeoff(mavproxy, mav, 10):
|
|
failed_test_msg = "takeoff failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# Fly a circle in super simple mode
|
|
progress("# Fly a circle in SUPER SIMPLE mode")
|
|
if not fly_super_simple(mavproxy, mav):
|
|
failed_test_msg = "fly_super_simple failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# RTL
|
|
progress("# RTL #")
|
|
if not fly_RTL(mavproxy, mav):
|
|
failed_test_msg = "fly_RTL after super simple mode failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# Takeoff
|
|
progress("# Takeoff")
|
|
if not takeoff(mavproxy, mav, 10):
|
|
failed_test_msg = "takeoff failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# Circle mode
|
|
progress("# Fly CIRCLE mode")
|
|
if not fly_circle(mavproxy, mav):
|
|
failed_test_msg = "fly_circle failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
# RTL
|
|
progress("#")
|
|
progress("########## Test RTL ##########")
|
|
progress("#")
|
|
if not fly_RTL(mavproxy, mav):
|
|
failed_test_msg = "fly_RTL after circle failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
progress("# Fly copter mission")
|
|
if not fly_auto_test(mavproxy, mav):
|
|
failed_test_msg = "fly_auto_test failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
else:
|
|
progress("Flew copter mission OK")
|
|
|
|
# wait for disarm
|
|
mav.motors_disarmed_wait()
|
|
|
|
if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/ArduCopter-log.bin")):
|
|
failed_test_msg = "log_download failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
except pexpect.TIMEOUT as e:
|
|
progress("Failed with timeout")
|
|
failed = True
|
|
|
|
mav.close()
|
|
util.pexpect_close(mavproxy)
|
|
util.pexpect_close(sitl)
|
|
|
|
valgrind_log = util.valgrind_log_filepath(binary=binary, model='+')
|
|
if os.path.exists(valgrind_log):
|
|
os.chmod(valgrind_log, 0o644)
|
|
shutil.copy(valgrind_log, util.reltopdir("../buildlogs/ArduCopter-valgrind.log"))
|
|
|
|
# [2014/05/07] FC Because I'm doing a cross machine build (source is on host, build is on guest VM) I cannot hard link
|
|
# This flag tells me that I need to copy the data out
|
|
if copy_tlog:
|
|
shutil.copy(logfile, buildlog)
|
|
|
|
if failed:
|
|
progress("FAILED: %s" % failed_test_msg)
|
|
return False
|
|
return True
|
|
|
|
|
|
def fly_CopterAVC(binary, viewerip=None, use_map=False, valgrind=False, gdb=False, frame=None, params=None, gdbserver=False, speedup=10):
|
|
"""Fly ArduCopter in SITL for AVC2013 mission."""
|
|
global homeloc
|
|
|
|
if frame is None:
|
|
frame = 'heli'
|
|
|
|
home = "%f,%f,%u,%u" % (AVCHOME.lat, AVCHOME.lng, AVCHOME.alt, AVCHOME.heading)
|
|
sitl = util.start_SITL(binary, wipe=True, model=frame, home=home, speedup=speedup)
|
|
mavproxy = util.start_MAVProxy_SITL('ArduCopter')
|
|
mavproxy.expect('Received [0-9]+ parameters')
|
|
|
|
# setup test parameters
|
|
if params is None:
|
|
params = vinfo.options["ArduCopter"]["frames"][frame]["default_params_filename"]
|
|
if not isinstance(params, list):
|
|
params = [params]
|
|
for x in params:
|
|
mavproxy.send("param load %s\n" % os.path.join(testdir, x))
|
|
mavproxy.expect('Loaded [0-9]+ parameters')
|
|
mavproxy.send("param set LOG_REPLAY 1\n")
|
|
mavproxy.send("param set LOG_DISARMED 1\n")
|
|
time.sleep(3)
|
|
|
|
# reboot with new parameters
|
|
util.pexpect_close(mavproxy)
|
|
util.pexpect_close(sitl)
|
|
|
|
sitl = util.start_SITL(binary, model='heli', home=home, speedup=speedup, valgrind=valgrind, gdb=gdb, gdbserver=gdbserver)
|
|
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=5'
|
|
if viewerip:
|
|
options += ' --out=%s:14550' % viewerip
|
|
if use_map:
|
|
options += ' --map'
|
|
mavproxy = util.start_MAVProxy_SITL('ArduCopter', options=options)
|
|
mavproxy.expect('Telemetry log: (\S+)\r\n')
|
|
logfile = mavproxy.match.group(1)
|
|
progress("LOGFILE %s" % logfile)
|
|
|
|
buildlog = util.reltopdir("../buildlogs/CopterAVC-test.tlog")
|
|
progress("buildlog=%s" % buildlog)
|
|
if os.path.exists(buildlog):
|
|
os.unlink(buildlog)
|
|
try:
|
|
os.link(logfile, buildlog)
|
|
except Exception:
|
|
pass
|
|
|
|
# the received parameters can come before or after the ready to fly message
|
|
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
|
|
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
|
|
|
|
util.expect_setup_callback(mavproxy, expect_callback)
|
|
|
|
expect_list_clear()
|
|
expect_list_extend([sitl, mavproxy])
|
|
|
|
if use_map:
|
|
mavproxy.send('map icon 40.072467969730496 -105.2314389590174\n')
|
|
mavproxy.send('map icon 40.072600990533829 -105.23146100342274\n')
|
|
|
|
# get a mavlink connection going
|
|
try:
|
|
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
|
|
except Exception as msg:
|
|
progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
|
|
raise
|
|
mav.message_hooks.append(message_hook)
|
|
mav.idle_hooks.append(idle_hook)
|
|
|
|
failed = False
|
|
failed_test_msg = "None"
|
|
|
|
try:
|
|
mav.wait_heartbeat()
|
|
set_rc_default(mavproxy)
|
|
set_rc(mavproxy, mav, 3, 1000)
|
|
homeloc = mav.location()
|
|
|
|
progress("Lowering rotor speed")
|
|
set_rc(mavproxy, mav, 8, 1000)
|
|
|
|
mavproxy.send('switch 6\n') # stabilize mode
|
|
wait_mode(mav, 'STABILIZE')
|
|
wait_ready_to_arm(mav)
|
|
|
|
# Arm
|
|
progress("# Arm motors")
|
|
if not arm_vehicle(mavproxy, mav):
|
|
failed_test_msg = "arm_motors failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
progress("Raising rotor speed")
|
|
set_rc(mavproxy, mav, 8, 2000)
|
|
|
|
progress("# Fly AVC mission")
|
|
if not fly_avc_test(mavproxy, mav):
|
|
failed_test_msg = "fly_avc_test failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
else:
|
|
progress("Flew AVC mission OK")
|
|
|
|
progress("Lowering rotor speed")
|
|
set_rc(mavproxy, mav, 8, 1000)
|
|
|
|
# mission includes disarm at end so should be ok to download logs now
|
|
if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/CopterAVC-log.bin")):
|
|
failed_test_msg = "log_download failed"
|
|
progress(failed_test_msg)
|
|
failed = True
|
|
|
|
except pexpect.TIMEOUT as failed_test_msg:
|
|
failed_test_msg = "Timeout"
|
|
failed = True
|
|
|
|
mav.close()
|
|
util.pexpect_close(mavproxy)
|
|
util.pexpect_close(sitl)
|
|
|
|
valgrind_log = util.valgrind_log_filepath(binary=binary, model='heli')
|
|
if os.path.exists(valgrind_log):
|
|
os.chmod(valgrind_log, 0o644)
|
|
shutil.copy(valgrind_log, util.reltopdir("../buildlogs/Helicopter-valgrind.log"))
|
|
|
|
if failed:
|
|
progress("FAILED: %s" % failed_test_msg)
|
|
return False
|
|
return True
|