mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-05 23:43:58 -04:00
autotest: update plane test with more modes
added tests for FBWB, STABILIZE, CRUISE, ACRO and CIRCLE
This commit is contained in:
Andrew Tridgell
parent
a4f49a28fb
commit
81bb4f26a1
@ -78,7 +78,7 @@ def fly_RTL(mavproxy, mav):
|
||||
wait_mode(mav, 'RTL')
|
||||
if not wait_location(mav, homeloc, accuracy=120,
|
||||
target_altitude=homeloc.alt+100, height_accuracy=20,
|
||||
timeout=90):
|
||||
timeout=180):
|
||||
return False
|
||||
print("RTL Complete")
|
||||
return True
|
||||
@ -89,6 +89,11 @@ def fly_LOITER(mavproxy, mav, num_circles=4):
|
||||
mavproxy.send('switch 3\n')
|
||||
mavproxy.send('loiter\n')
|
||||
wait_mode(mav, 'LOITER')
|
||||
|
||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
initial_alt = m.alt
|
||||
print("Initial altitude %u\n" % initial_alt)
|
||||
|
||||
while num_circles > 0:
|
||||
if not wait_heading(mav, 0, accuracy=10, timeout=60):
|
||||
return False
|
||||
@ -96,9 +101,53 @@ def fly_LOITER(mavproxy, mav, num_circles=4):
|
||||
return False
|
||||
num_circles -= 1
|
||||
print("Loiter %u circles left" % num_circles)
|
||||
|
||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
final_alt = m.alt
|
||||
print("Final altitude %u\n" % final_alt)
|
||||
|
||||
mavproxy.send('mode FBWA\n')
|
||||
wait_mode(mav, 'FBWA')
|
||||
|
||||
if abs(final_alt - initial_alt) > 20:
|
||||
print("Failed to maintain altitude")
|
||||
return False
|
||||
|
||||
print("Completed Loiter OK")
|
||||
return True
|
||||
|
||||
def fly_CIRCLE(mavproxy, mav, num_circles=1):
|
||||
'''circle where we are'''
|
||||
print("Testing CIRCLE for %u turns" % num_circles)
|
||||
mavproxy.send('mode CIRCLE\n')
|
||||
wait_mode(mav, 'CIRCLE')
|
||||
|
||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
initial_alt = m.alt
|
||||
print("Initial altitude %u\n" % initial_alt)
|
||||
|
||||
while num_circles > 0:
|
||||
if not wait_heading(mav, 0, accuracy=10, timeout=60):
|
||||
return False
|
||||
if not wait_heading(mav, 180, accuracy=10, timeout=60):
|
||||
return False
|
||||
num_circles -= 1
|
||||
print("CIRCLE %u circles left" % num_circles)
|
||||
|
||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
final_alt = m.alt
|
||||
print("Final altitude %u\n" % final_alt)
|
||||
|
||||
mavproxy.send('mode FBWA\n')
|
||||
wait_mode(mav, 'FBWA')
|
||||
|
||||
if abs(final_alt - initial_alt) > 20:
|
||||
print("Failed to maintain altitude")
|
||||
return False
|
||||
|
||||
print("Completed CIRCLE OK")
|
||||
return True
|
||||
|
||||
|
||||
def wait_level_flight(mavproxy, mav, accuracy=5, timeout=30):
|
||||
'''wait for level flight'''
|
||||
@ -116,9 +165,9 @@ def wait_level_flight(mavproxy, mav, accuracy=5, timeout=30):
|
||||
return False
|
||||
|
||||
|
||||
def change_altitude(mavproxy, mav, altitude, accuracy=10):
|
||||
def change_altitude(mavproxy, mav, altitude, accuracy=30):
|
||||
'''get to a given altitude'''
|
||||
mavproxy.send('switch 4\n')
|
||||
mavproxy.send('mode FBWA\n')
|
||||
wait_mode(mav, 'FBWA')
|
||||
alt_error = mav.messages['VFR_HUD'].alt - altitude
|
||||
if alt_error > 0:
|
||||
@ -136,7 +185,7 @@ def axial_left_roll(mavproxy, mav, count=1):
|
||||
'''fly a left axial roll'''
|
||||
# full throttle!
|
||||
mavproxy.send('rc 3 2000\n')
|
||||
if not change_altitude(mavproxy, mav, homeloc.alt+200):
|
||||
if not change_altitude(mavproxy, mav, homeloc.alt+300):
|
||||
return False
|
||||
|
||||
# fly the roll in manual
|
||||
@ -146,11 +195,11 @@ def axial_left_roll(mavproxy, mav, count=1):
|
||||
while count > 0:
|
||||
print("Starting roll")
|
||||
mavproxy.send('rc 1 1000\n')
|
||||
if not wait_roll(mav, -150, accuracy=45):
|
||||
if not wait_roll(mav, -150, accuracy=90):
|
||||
return False
|
||||
if not wait_roll(mav, 150, accuracy=45):
|
||||
if not wait_roll(mav, 150, accuracy=90):
|
||||
return False
|
||||
if not wait_roll(mav, 0, accuracy=45):
|
||||
if not wait_roll(mav, 0, accuracy=90):
|
||||
return False
|
||||
count -= 1
|
||||
|
||||
@ -166,7 +215,7 @@ def inside_loop(mavproxy, mav, count=1):
|
||||
'''fly a inside loop'''
|
||||
# full throttle!
|
||||
mavproxy.send('rc 3 2000\n')
|
||||
if not change_altitude(mavproxy, mav, homeloc.alt+200):
|
||||
if not change_altitude(mavproxy, mav, homeloc.alt+300):
|
||||
return False
|
||||
|
||||
# fly the loop in manual
|
||||
@ -190,6 +239,148 @@ def inside_loop(mavproxy, mav, count=1):
|
||||
return wait_level_flight(mavproxy, mav)
|
||||
|
||||
|
||||
def test_stabilize(mavproxy, mav, count=1):
|
||||
'''fly stabilize mode'''
|
||||
# full throttle!
|
||||
mavproxy.send('rc 3 2000\n')
|
||||
mavproxy.send('rc 2 1300\n')
|
||||
if not change_altitude(mavproxy, mav, homeloc.alt+300):
|
||||
return False
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
|
||||
mavproxy.send("mode STABILIZE\n")
|
||||
wait_mode(mav, 'STABILIZE')
|
||||
|
||||
count = 1
|
||||
while count > 0:
|
||||
print("Starting roll")
|
||||
mavproxy.send('rc 1 2000\n')
|
||||
if not wait_roll(mav, -150, accuracy=90):
|
||||
return False
|
||||
if not wait_roll(mav, 150, accuracy=90):
|
||||
return False
|
||||
if not wait_roll(mav, 0, accuracy=90):
|
||||
return False
|
||||
count -= 1
|
||||
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
if not wait_roll(mav, 0, accuracy=5):
|
||||
return False
|
||||
|
||||
# back to FBWA
|
||||
mavproxy.send('mode FBWA\n')
|
||||
wait_mode(mav, 'FBWA')
|
||||
mavproxy.send('rc 3 1700\n')
|
||||
return wait_level_flight(mavproxy, mav)
|
||||
|
||||
def test_acro(mavproxy, mav, count=1):
|
||||
'''fly ACRO mode'''
|
||||
# full throttle!
|
||||
mavproxy.send('rc 3 2000\n')
|
||||
mavproxy.send('rc 2 1300\n')
|
||||
if not change_altitude(mavproxy, mav, homeloc.alt+300):
|
||||
return False
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
|
||||
mavproxy.send("mode ACRO\n")
|
||||
wait_mode(mav, 'ACRO')
|
||||
|
||||
count = 1
|
||||
while count > 0:
|
||||
print("Starting roll")
|
||||
mavproxy.send('rc 1 1000\n')
|
||||
if not wait_roll(mav, -150, accuracy=90):
|
||||
return False
|
||||
if not wait_roll(mav, 150, accuracy=90):
|
||||
return False
|
||||
if not wait_roll(mav, 0, accuracy=90):
|
||||
return False
|
||||
count -= 1
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
|
||||
# back to FBWA
|
||||
mavproxy.send('mode FBWA\n')
|
||||
wait_mode(mav, 'FBWA')
|
||||
|
||||
wait_level_flight(mavproxy, mav)
|
||||
|
||||
mavproxy.send("mode ACRO\n")
|
||||
wait_mode(mav, 'ACRO')
|
||||
|
||||
count = 2
|
||||
while count > 0:
|
||||
print("Starting loop")
|
||||
mavproxy.send('rc 2 1000\n')
|
||||
if not wait_pitch(mav, -60, accuracy=20):
|
||||
return False
|
||||
if not wait_pitch(mav, 0, accuracy=20):
|
||||
return False
|
||||
count -= 1
|
||||
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
|
||||
# back to FBWA
|
||||
mavproxy.send('mode FBWA\n')
|
||||
wait_mode(mav, 'FBWA')
|
||||
mavproxy.send('rc 3 1700\n')
|
||||
return wait_level_flight(mavproxy, mav)
|
||||
|
||||
|
||||
def test_FBWB(mavproxy, mav, count=1, mode='FBWB'):
|
||||
'''fly FBWB or CRUISE mode'''
|
||||
mavproxy.send("mode %s\n" % mode)
|
||||
wait_mode(mav, mode)
|
||||
mavproxy.send('rc 3 1700\n')
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
|
||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
initial_alt = m.alt
|
||||
print("Initial altitude %u\n" % initial_alt)
|
||||
|
||||
print("Flying right circuit")
|
||||
# do 4 turns
|
||||
for i in range(0,4):
|
||||
# hard left
|
||||
print("Starting turn %u" % i)
|
||||
mavproxy.send('rc 1 1800\n')
|
||||
if not wait_heading(mav, 0 + (90*i), accuracy=20, timeout=60):
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
return False
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
print("Starting leg %u" % i)
|
||||
if not wait_distance(mav, 100, accuracy=20):
|
||||
return False
|
||||
print("Circuit complete")
|
||||
|
||||
print("Flying rudder left circuit")
|
||||
# do 4 turns
|
||||
for i in range(0,4):
|
||||
# hard left
|
||||
print("Starting turn %u" % i)
|
||||
mavproxy.send('rc 4 1700\n')
|
||||
if not wait_heading(mav, 360 - (90*i), accuracy=20, timeout=60):
|
||||
mavproxy.send('rc 4 1500\n')
|
||||
return False
|
||||
mavproxy.send('rc 4 1500\n')
|
||||
print("Starting leg %u" % i)
|
||||
if not wait_distance(mav, 100, accuracy=20):
|
||||
return False
|
||||
print("Circuit complete")
|
||||
|
||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
final_alt = m.alt
|
||||
print("Final altitude %u\n" % final_alt)
|
||||
|
||||
# back to FBWA
|
||||
mavproxy.send('mode FBWA\n')
|
||||
wait_mode(mav, 'FBWA')
|
||||
|
||||
if abs(final_alt - initial_alt) > 20:
|
||||
print("Failed to maintain altitude")
|
||||
return False
|
||||
|
||||
return wait_level_flight(mavproxy, mav)
|
||||
|
||||
|
||||
def setup_rc(mavproxy):
|
||||
'''setup RC override control'''
|
||||
@ -313,12 +504,27 @@ def fly_ArduPlane(viewerip=None, map=False):
|
||||
if not inside_loop(mavproxy, mav):
|
||||
print("Failed inside loop")
|
||||
failed = True
|
||||
if not test_stabilize(mavproxy, mav):
|
||||
print("Failed stabilize test")
|
||||
failed = True
|
||||
if not test_acro(mavproxy, mav):
|
||||
print("Failed ACRO test")
|
||||
failed = True
|
||||
if not test_FBWB(mavproxy, mav):
|
||||
print("Failed FBWB test")
|
||||
failed = True
|
||||
if not test_FBWB(mavproxy, mav, mode='CRUISE'):
|
||||
print("Failed CRUISE test")
|
||||
failed = True
|
||||
if not fly_RTL(mavproxy, mav):
|
||||
print("Failed RTL")
|
||||
failed = True
|
||||
if not fly_LOITER(mavproxy, mav):
|
||||
print("Failed LOITER")
|
||||
failed = True
|
||||
if not fly_CIRCLE(mavproxy, mav):
|
||||
print("Failed CIRCLE")
|
||||
failed = True
|
||||
if not fly_mission(mavproxy, mav, os.path.join(testdir, "ap1.txt"), height_accuracy = 10,
|
||||
target_altitude=homeloc.alt+100):
|
||||
print("Failed mission")
|
||||
|
||||
@ -114,6 +114,7 @@ def wait_pitch(mav, pitch, accuracy, timeout=30):
|
||||
def wait_heading(mav, heading, accuracy=5, timeout=30):
|
||||
'''wait for a given heading'''
|
||||
tstart = time.time()
|
||||
print("Waiting for heading %u with accuracy %u" % (heading, accuracy))
|
||||
while time.time() < tstart + timeout:
|
||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
print("Heading %u" % m.heading)
|
||||
@ -212,7 +213,9 @@ def save_wp(mavproxy, mav):
|
||||
|
||||
def wait_mode(mav, mode):
|
||||
'''wait for a flight mode to be engaged'''
|
||||
mav.recv_match(condition='MAV.flightmode=="%s"' % mode, blocking=True)
|
||||
print("Waiting for mode %s" % mode)
|
||||
mav.recv_match(condition='MAV.flightmode.upper()=="%s".upper()' % mode, blocking=True)
|
||||
print("Got mode %s" % mode)
|
||||
|
||||
def mission_count(filename):
|
||||
'''load a mission from a file and return number of waypoints'''
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
#!/bin/bash
|
||||
|
||||
export PATH=/usr/local/bin:$HOME/prefix/bin:$HOME/APM/px4/gcc-arm-none-eabi-4_6-2012q2/bin:$PATH
|
||||
export PATH=$HOME/.local/bin:/usr/local/bin:$HOME/prefix/bin:$HOME/APM/px4/gcc-arm-none-eabi-4_6-2012q2/bin:$PATH
|
||||
export PYTHONUNBUFFERED=1
|
||||
export PYTHONPATH=$HOME/APM
|
||||
export PX4_ROOT=$HOME/APM/px4/PX4Firmware
|
||||
@ -88,11 +88,17 @@ git fetch origin
|
||||
git reset --hard origin/master
|
||||
popd
|
||||
|
||||
for d in MAVProxy mavlink; do
|
||||
pushd $d
|
||||
git pr
|
||||
popd
|
||||
done
|
||||
echo "Updating pymavlink"
|
||||
pushd mavlink/pymavlink
|
||||
git pr
|
||||
python setup.py build install --user
|
||||
popd
|
||||
|
||||
echo "Updating MAVProxy"
|
||||
pushd MAVProxy
|
||||
git pr
|
||||
python setup.py build install --user
|
||||
popd
|
||||
|
||||
githash=$(cd APM && git rev-parse HEAD)
|
||||
hdate=$(date +"%Y-%m-%d-%H:%m")
|
||||
|
||||
Loading…
Reference in New Issue
Block a user