You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1482 lines
55 KiB
1482 lines
55 KiB
#!/usr/bin/env python |
|
|
|
# Fly ArduPlane in SITL |
|
from __future__ import print_function |
|
import math |
|
import os |
|
|
|
from pymavlink import quaternion |
|
from pymavlink import mavutil |
|
|
|
from common import AutoTest |
|
from common import AutoTestTimeoutException |
|
from common import NotAchievedException |
|
from common import PreconditionFailedException |
|
|
|
# get location of scripts |
|
testdir = os.path.dirname(os.path.realpath(__file__)) |
|
SITL_START_LOCATION = mavutil.location(-35.362938, 149.165085, 585, 354) |
|
WIND = "0,180,0.2" # speed,direction,variance |
|
|
|
|
|
class AutoTestPlane(AutoTest): |
|
@staticmethod |
|
def get_not_armable_mode_list(): |
|
return [] |
|
|
|
@staticmethod |
|
def get_not_disarmed_settable_modes_list(): |
|
return ["FOLLOW"] |
|
|
|
@staticmethod |
|
def get_no_position_not_settable_modes_list(): |
|
return [] |
|
|
|
@staticmethod |
|
def get_position_armable_modes_list(): |
|
return ["GUIDED", "AUTO"] |
|
|
|
@staticmethod |
|
def get_normal_armable_modes_list(): |
|
return ["MANUAL", "STABILIZE", "ACRO"] |
|
|
|
def log_name(self): |
|
return "ArduPlane" |
|
|
|
def test_filepath(self): |
|
return os.path.realpath(__file__) |
|
|
|
def sitl_start_location(self): |
|
return SITL_START_LOCATION |
|
|
|
def defaults_filepath(self): |
|
return os.path.join(testdir, 'default_params/plane-jsbsim.parm') |
|
|
|
def default_frame(self): |
|
return "plane-elevrev" |
|
|
|
def apply_defaultfile_parameters(self): |
|
# plane passes in a defaults_file in place of applying |
|
# parameters afterwards. |
|
pass |
|
|
|
def is_plane(self): |
|
return True |
|
|
|
def get_stick_arming_channel(self): |
|
return int(self.get_parameter("RCMAP_YAW")) |
|
|
|
def get_disarm_delay(self): |
|
return int(self.get_parameter("LAND_DISARMDELAY")) |
|
|
|
def set_autodisarm_delay(self, delay): |
|
self.set_parameter("LAND_DISARMDELAY", delay) |
|
|
|
def arming_test_mission(self): |
|
return os.path.join(testdir, "ArduPlane-Missions", "test_arming.txt") |
|
|
|
def takeoff(self, alt=150, alt_max=None, relative=True): |
|
"""Takeoff to altitude.""" |
|
|
|
if alt_max is None: |
|
alt_max = alt + 30 |
|
|
|
self.change_mode("FBWA") |
|
|
|
self.wait_ready_to_arm() |
|
self.arm_vehicle() |
|
|
|
# some rudder to counteract the prop torque |
|
self.set_rc(4, 1700) |
|
|
|
# some up elevator to keep the tail down |
|
self.set_rc(2, 1200) |
|
|
|
# get it moving a bit first |
|
self.set_rc(3, 1300) |
|
self.wait_groundspeed(6, 100) |
|
|
|
# a bit faster again, straighten rudder |
|
self.set_rc(3, 1600) |
|
self.set_rc(4, 1500) |
|
self.wait_groundspeed(12, 100) |
|
|
|
# hit the gas harder now, and give it some more elevator |
|
self.set_rc(2, 1100) |
|
self.set_rc(3, 2000) |
|
|
|
# gain a bit of altitude |
|
self.wait_altitude(alt, alt_max, timeout=30, relative=relative) |
|
|
|
# level off |
|
self.set_rc(2, 1500) |
|
|
|
self.progress("TAKEOFF COMPLETE") |
|
|
|
def fly_left_circuit(self): |
|
"""Fly a left circuit, 200m on a side.""" |
|
self.mavproxy.send('switch 4\n') |
|
self.wait_mode('FBWA') |
|
self.set_rc(3, 2000) |
|
self.wait_level_flight() |
|
|
|
self.progress("Flying left circuit") |
|
# do 4 turns |
|
for i in range(0, 4): |
|
# hard left |
|
self.progress("Starting turn %u" % i) |
|
self.set_rc(1, 1000) |
|
self.wait_heading(270 - (90*i), accuracy=10) |
|
self.set_rc(1, 1500) |
|
self.progress("Starting leg %u" % i) |
|
self.wait_distance(100, accuracy=20) |
|
self.progress("Circuit complete") |
|
|
|
def fly_RTL(self): |
|
"""Fly to home.""" |
|
self.progress("Flying home in RTL") |
|
self.mavproxy.send('switch 2\n') |
|
self.wait_mode('RTL') |
|
self.wait_location(self.homeloc, |
|
accuracy=120, |
|
target_altitude=self.homeloc.alt+100, |
|
height_accuracy=20, |
|
timeout=180) |
|
self.progress("RTL Complete") |
|
|
|
def fly_LOITER(self, num_circles=4): |
|
"""Loiter where we are.""" |
|
self.progress("Testing LOITER for %u turns" % num_circles) |
|
self.mavproxy.send('loiter\n') |
|
self.wait_mode('LOITER') |
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True) |
|
initial_alt = m.alt |
|
self.progress("Initial altitude %u\n" % initial_alt) |
|
|
|
while num_circles > 0: |
|
self.wait_heading(0, accuracy=10, timeout=60) |
|
self.wait_heading(180, accuracy=10, timeout=60) |
|
num_circles -= 1 |
|
self.progress("Loiter %u circles left" % num_circles) |
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True) |
|
final_alt = m.alt |
|
self.progress("Final altitude %u initial %u\n" % |
|
(final_alt, initial_alt)) |
|
|
|
self.mavproxy.send('mode FBWA\n') |
|
self.wait_mode('FBWA') |
|
|
|
if abs(final_alt - initial_alt) > 20: |
|
raise NotAchievedException("Failed to maintain altitude") |
|
|
|
self.progress("Completed Loiter OK") |
|
|
|
def fly_CIRCLE(self, num_circles=1): |
|
"""Circle where we are.""" |
|
self.progress("Testing CIRCLE for %u turns" % num_circles) |
|
self.mavproxy.send('mode CIRCLE\n') |
|
self.wait_mode('CIRCLE') |
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True) |
|
initial_alt = m.alt |
|
self.progress("Initial altitude %u\n" % initial_alt) |
|
|
|
while num_circles > 0: |
|
self.wait_heading(0, accuracy=10, timeout=60) |
|
self.wait_heading(180, accuracy=10, timeout=60) |
|
num_circles -= 1 |
|
self.progress("CIRCLE %u circles left" % num_circles) |
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True) |
|
final_alt = m.alt |
|
self.progress("Final altitude %u initial %u\n" % |
|
(final_alt, initial_alt)) |
|
|
|
self.mavproxy.send('mode FBWA\n') |
|
self.wait_mode('FBWA') |
|
|
|
if abs(final_alt - initial_alt) > 20: |
|
raise NotAchievedException("Failed to maintain altitude") |
|
|
|
self.progress("Completed CIRCLE OK") |
|
|
|
def wait_level_flight(self, accuracy=5, timeout=30): |
|
"""Wait for level flight.""" |
|
tstart = self.get_sim_time() |
|
self.progress("Waiting for level flight") |
|
self.set_rc(1, 1500) |
|
self.set_rc(2, 1500) |
|
self.set_rc(4, 1500) |
|
while self.get_sim_time_cached() < tstart + timeout: |
|
m = self.mav.recv_match(type='ATTITUDE', blocking=True) |
|
roll = math.degrees(m.roll) |
|
pitch = math.degrees(m.pitch) |
|
self.progress("Roll=%.1f Pitch=%.1f" % (roll, pitch)) |
|
if math.fabs(roll) <= accuracy and math.fabs(pitch) <= accuracy: |
|
self.progress("Attained level flight") |
|
return |
|
raise NotAchievedException("Failed to attain level flight") |
|
|
|
def change_altitude(self, altitude, accuracy=30): |
|
"""Get to a given altitude.""" |
|
self.mavproxy.send('mode FBWA\n') |
|
self.wait_mode('FBWA') |
|
alt_error = self.mav.messages['VFR_HUD'].alt - altitude |
|
if alt_error > 0: |
|
self.set_rc(2, 2000) |
|
else: |
|
self.set_rc(2, 1000) |
|
self.wait_altitude(altitude-accuracy/2, altitude+accuracy/2) |
|
self.set_rc(2, 1500) |
|
self.progress("Reached target altitude at %u" % |
|
self.mav.messages['VFR_HUD'].alt) |
|
return self.wait_level_flight() |
|
|
|
def axial_left_roll(self, count=1): |
|
"""Fly a left axial roll.""" |
|
# full throttle! |
|
self.set_rc(3, 2000) |
|
self.change_altitude(self.homeloc.alt+300) |
|
|
|
# fly the roll in manual |
|
self.mavproxy.send('switch 6\n') |
|
self.wait_mode('MANUAL') |
|
|
|
while count > 0: |
|
self.progress("Starting roll") |
|
self.set_rc(1, 1000) |
|
try: |
|
self.wait_roll(-150, accuracy=90) |
|
self.wait_roll(150, accuracy=90) |
|
self.wait_roll(0, accuracy=90) |
|
except Exception as e: |
|
self.set_rc(1, 1500) |
|
raise e |
|
count -= 1 |
|
|
|
# back to FBWA |
|
self.set_rc(1, 1500) |
|
self.mavproxy.send('switch 4\n') |
|
self.wait_mode('FBWA') |
|
self.set_rc(3, 1700) |
|
return self.wait_level_flight() |
|
|
|
def inside_loop(self, count=1): |
|
"""Fly a inside loop.""" |
|
# full throttle! |
|
self.set_rc(3, 2000) |
|
self.change_altitude(self.homeloc.alt+300) |
|
# fly the loop in manual |
|
self.mavproxy.send('switch 6\n') |
|
self.wait_mode('MANUAL') |
|
|
|
while count > 0: |
|
self.progress("Starting loop") |
|
self.set_rc(2, 1000) |
|
self.wait_pitch(-60, accuracy=20) |
|
self.wait_pitch(0, accuracy=20) |
|
count -= 1 |
|
|
|
# back to FBWA |
|
self.set_rc(2, 1500) |
|
self.mavproxy.send('switch 4\n') |
|
self.wait_mode('FBWA') |
|
self.set_rc(3, 1700) |
|
return self.wait_level_flight() |
|
|
|
def set_attitude_target(self, tolerance=10): |
|
"""Test setting of attitude target in guided mode.""" |
|
self.change_mode("GUIDED") |
|
# self.set_parameter("STALL_PREVENTION", 0) |
|
|
|
state_roll_over = "roll-over" |
|
state_stabilize_roll = "stabilize-roll" |
|
state_hold = "hold" |
|
state_roll_back = "roll-back" |
|
state_done = "done" |
|
|
|
tstart = self.get_sim_time() |
|
|
|
try: |
|
state = state_roll_over |
|
while state != state_done: |
|
|
|
m = self.mav.recv_match(type='ATTITUDE', |
|
blocking=True, |
|
timeout=0.1) |
|
now = self.get_sim_time_cached() |
|
if now - tstart > 20: |
|
raise AutoTestTimeoutException("Manuevers not completed") |
|
if m is None: |
|
continue |
|
|
|
r = math.degrees(m.roll) |
|
if state == state_roll_over: |
|
target_roll_degrees = 60 |
|
if abs(r - target_roll_degrees) < tolerance: |
|
state = state_stabilize_roll |
|
stabilize_start = now |
|
elif state == state_stabilize_roll: |
|
# just give it a little time to sort it self out |
|
if now - stabilize_start > 2: |
|
state = state_hold |
|
hold_start = now |
|
elif state == state_hold: |
|
target_roll_degrees = 60 |
|
if now - hold_start > tolerance: |
|
state = state_roll_back |
|
if abs(r - target_roll_degrees) > tolerance: |
|
raise NotAchievedException("Failed to hold attitude") |
|
elif state == state_roll_back: |
|
target_roll_degrees = 0 |
|
if abs(r - target_roll_degrees) < tolerance: |
|
state = state_done |
|
else: |
|
raise ValueError("Unknown state %s" % str(state)) |
|
|
|
m_nav = self.mav.messages['NAV_CONTROLLER_OUTPUT'] |
|
self.progress("%s Roll: %f desired=%f set=%f" % |
|
(state, r, m_nav.nav_roll, target_roll_degrees)) |
|
|
|
time_boot_millis = 0 # FIXME |
|
target_system = 1 # FIXME |
|
target_component = 1 # FIXME |
|
type_mask = 0b10000001 ^ 0xFF # FIXME |
|
# attitude in radians: |
|
q = quaternion.Quaternion([math.radians(target_roll_degrees), |
|
0, |
|
0]) |
|
roll_rate_radians = 0.5 |
|
pitch_rate_radians = 0 |
|
yaw_rate_radians = 0 |
|
thrust = 1.0 |
|
self.mav.mav.set_attitude_target_send(time_boot_millis, |
|
target_system, |
|
target_component, |
|
type_mask, |
|
q, |
|
roll_rate_radians, |
|
pitch_rate_radians, |
|
yaw_rate_radians, |
|
thrust) |
|
except Exception as e: |
|
self.mavproxy.send('mode FBWA\n') |
|
self.wait_mode('FBWA') |
|
self.set_rc(3, 1700) |
|
raise e |
|
|
|
# back to FBWA |
|
self.mavproxy.send('mode FBWA\n') |
|
self.wait_mode('FBWA') |
|
self.set_rc(3, 1700) |
|
self.wait_level_flight() |
|
|
|
def test_stabilize(self, count=1): |
|
"""Fly stabilize mode.""" |
|
# full throttle! |
|
self.set_rc(3, 2000) |
|
self.set_rc(2, 1300) |
|
self.change_altitude(self.homeloc.alt+300) |
|
self.set_rc(2, 1500) |
|
|
|
self.mavproxy.send("mode STABILIZE\n") |
|
self.wait_mode('STABILIZE') |
|
|
|
while count > 0: |
|
self.progress("Starting roll") |
|
self.set_rc(1, 2000) |
|
self.wait_roll(-150, accuracy=90) |
|
self.wait_roll(150, accuracy=90) |
|
self.wait_roll(0, accuracy=90) |
|
count -= 1 |
|
|
|
self.set_rc(1, 1500) |
|
self.wait_roll(0, accuracy=5) |
|
|
|
# back to FBWA |
|
self.mavproxy.send('mode FBWA\n') |
|
self.wait_mode('FBWA') |
|
self.set_rc(3, 1700) |
|
return self.wait_level_flight() |
|
|
|
def test_acro(self, count=1): |
|
"""Fly ACRO mode.""" |
|
# full throttle! |
|
self.set_rc(3, 2000) |
|
self.set_rc(2, 1300) |
|
self.change_altitude(self.homeloc.alt+300) |
|
self.set_rc(2, 1500) |
|
|
|
self.mavproxy.send("mode ACRO\n") |
|
self.wait_mode('ACRO') |
|
|
|
while count > 0: |
|
self.progress("Starting roll") |
|
self.set_rc(1, 1000) |
|
self.wait_roll(-150, accuracy=90) |
|
self.wait_roll(150, accuracy=90) |
|
self.wait_roll(0, accuracy=90) |
|
count -= 1 |
|
self.set_rc(1, 1500) |
|
|
|
# back to FBWA |
|
self.mavproxy.send('mode FBWA\n') |
|
self.wait_mode('FBWA') |
|
|
|
self.wait_level_flight() |
|
|
|
self.mavproxy.send("mode ACRO\n") |
|
self.wait_mode('ACRO') |
|
|
|
count = 2 |
|
while count > 0: |
|
self.progress("Starting loop") |
|
self.set_rc(2, 1000) |
|
self.wait_pitch(-60, accuracy=20) |
|
self.wait_pitch(0, accuracy=20) |
|
count -= 1 |
|
|
|
self.set_rc(2, 1500) |
|
|
|
# back to FBWA |
|
self.mavproxy.send('mode FBWA\n') |
|
self.wait_mode('FBWA') |
|
self.set_rc(3, 1700) |
|
return self.wait_level_flight() |
|
|
|
def test_FBWB(self, mode='FBWB'): |
|
"""Fly FBWB or CRUISE mode.""" |
|
self.mavproxy.send("mode %s\n" % mode) |
|
self.wait_mode(mode) |
|
self.set_rc(3, 1700) |
|
self.set_rc(2, 1500) |
|
|
|
# lock in the altitude by asking for an altitude change then releasing |
|
self.set_rc(2, 1000) |
|
self.wait_distance(50, accuracy=20) |
|
self.set_rc(2, 1500) |
|
self.wait_distance(50, accuracy=20) |
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True) |
|
initial_alt = m.alt |
|
self.progress("Initial altitude %u\n" % initial_alt) |
|
|
|
self.progress("Flying right circuit") |
|
# do 4 turns |
|
for i in range(0, 4): |
|
# hard left |
|
self.progress("Starting turn %u" % i) |
|
self.set_rc(1, 1800) |
|
try: |
|
self.wait_heading(0 + (90*i), accuracy=20, timeout=60) |
|
except Exception as e: |
|
self.set_rc(1, 1500) |
|
raise e |
|
self.set_rc(1, 1500) |
|
self.progress("Starting leg %u" % i) |
|
self.wait_distance(100, accuracy=20) |
|
self.progress("Circuit complete") |
|
|
|
self.progress("Flying rudder left circuit") |
|
# do 4 turns |
|
for i in range(0, 4): |
|
# hard left |
|
self.progress("Starting turn %u" % i) |
|
self.set_rc(4, 1900) |
|
try: |
|
self.wait_heading(360 - (90*i), accuracy=20, timeout=60) |
|
except Exception as e: |
|
self.set_rc(4, 1500) |
|
raise e |
|
self.set_rc(4, 1500) |
|
self.progress("Starting leg %u" % i) |
|
self.wait_distance(100, accuracy=20) |
|
self.progress("Circuit complete") |
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True) |
|
final_alt = m.alt |
|
self.progress("Final altitude %u initial %u\n" % |
|
(final_alt, initial_alt)) |
|
|
|
# back to FBWA |
|
self.mavproxy.send('mode FBWA\n') |
|
self.wait_mode('FBWA') |
|
|
|
if abs(final_alt - initial_alt) > 20: |
|
raise NotAchievedException("Failed to maintain altitude") |
|
|
|
return self.wait_level_flight() |
|
|
|
def fly_mission(self, filename): |
|
"""Fly a mission from a file.""" |
|
self.progress("Flying mission %s" % filename) |
|
self.load_mission(filename) |
|
self.mavproxy.send('switch 1\n') # auto mode |
|
self.wait_mode('AUTO') |
|
self.wait_waypoint(1, 7, max_dist=60) |
|
self.wait_groundspeed(0, 0.5, timeout=60) |
|
self.mavproxy.expect("Auto disarmed") |
|
self.progress("Mission OK") |
|
|
|
def fly_do_reposition(self): |
|
self.progress("Takeoff") |
|
self.takeoff(alt=50) |
|
self.set_rc(3, 1500) |
|
self.progress("Entering guided and flying somewhere constant") |
|
self.change_mode("GUIDED") |
|
loc = self.mav.location() |
|
self.location_offset_ne(loc, 500, 500) |
|
|
|
new_alt = 100 |
|
self.run_cmd_int( |
|
mavutil.mavlink.MAV_CMD_DO_REPOSITION, |
|
0, |
|
0, |
|
0, |
|
0, |
|
int(loc.lat*1e7), |
|
int(loc.lng*1e7), |
|
new_alt, # alt |
|
frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, |
|
) |
|
self.wait_altitude(new_alt-10, new_alt, timeout=30, relative=True) |
|
|
|
self.fly_home_land_and_disarm() |
|
|
|
def fly_deepstall(self): |
|
# self.fly_deepstall_absolute() |
|
self.fly_deepstall_relative() |
|
|
|
def fly_deepstall_absolute(self): |
|
self.start_subtest("DeepStall Relative Absolute") |
|
self.set_parameter("LAND_TYPE", 1) |
|
deepstall_elevator_pwm = 1661 |
|
self.set_parameter("LAND_DS_ELEV_PWM", deepstall_elevator_pwm) |
|
self.load_mission("plane-deepstall-mission.txt") |
|
self.change_mode("AUTO") |
|
self.wait_ready_to_arm() |
|
self.arm_vehicle() |
|
self.progress("Waiting for deepstall messages") |
|
|
|
self.wait_text("Deepstall: Entry: ", timeout=240) |
|
|
|
# assume elevator is on channel 2: |
|
self.wait_servo_channel_value(2, deepstall_elevator_pwm) |
|
|
|
self.disarm_wait(timeout=120) |
|
|
|
self.progress("Flying home") |
|
self.takeoff(10) |
|
self.set_parameter("LAND_TYPE", 0) |
|
self.fly_home_land_and_disarm() |
|
|
|
def fly_deepstall_relative(self): |
|
self.start_subtest("DeepStall Relative") |
|
self.set_parameter("LAND_TYPE", 1) |
|
deepstall_elevator_pwm = 1661 |
|
self.set_parameter("LAND_DS_ELEV_PWM", deepstall_elevator_pwm) |
|
self.load_mission("plane-deepstall-relative-mission.txt") |
|
self.change_mode("AUTO") |
|
self.wait_ready_to_arm() |
|
self.arm_vehicle() |
|
self.progress("Waiting for deepstall messages") |
|
|
|
self.wait_text("Deepstall: Entry: ", timeout=240) |
|
|
|
# assume elevator is on channel 2: |
|
self.wait_servo_channel_value(2, deepstall_elevator_pwm) |
|
|
|
self.disarm_wait(timeout=120) |
|
|
|
self.progress("Flying home") |
|
self.takeoff(10) |
|
self.set_parameter("LAND_TYPE", 0) |
|
self.fly_home_land_and_disarm() |
|
|
|
def fly_do_change_speed(self): |
|
# the following lines ensure we revert these parameter values |
|
# - DO_CHANGE_AIRSPEED is a permanent vehicle change! |
|
self.set_parameter("TRIM_ARSPD_CM", self.get_parameter("TRIM_ARSPD_CM")) |
|
self.set_parameter("MIN_GNDSPD_CM", self.get_parameter("MIN_GNDSPD_CM")) |
|
|
|
self.progress("Takeoff") |
|
self.takeoff(alt=100) |
|
self.set_rc(3, 1500) |
|
# ensure we know what the airspeed is: |
|
self.progress("Entering guided and flying somewhere constant") |
|
self.change_mode("GUIDED") |
|
self.run_cmd_int( |
|
mavutil.mavlink.MAV_CMD_DO_REPOSITION, |
|
0, |
|
0, |
|
0, |
|
0, |
|
12345, # lat*1e7 |
|
12345, # lon*1e7 |
|
100 # alt |
|
) |
|
self.delay_sim_time(10) |
|
self.progress("Ensuring initial speed is known and relatively constant") |
|
initial_speed = 21.5; |
|
timeout = 10 |
|
tstart = self.get_sim_time() |
|
while True: |
|
if self.get_sim_time_cached() - tstart > timeout: |
|
break |
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True) |
|
self.progress("GroundSpeed: %f want=%f" % |
|
(m.groundspeed, initial_speed)) |
|
if abs(initial_speed - m.groundspeed) > 1: |
|
raise NotAchievedException("Initial speed not as expected (want=%f got=%f" % (initial_speed, m.groundspeed)) |
|
|
|
self.progress("Setting groundspeed") |
|
new_target_groundspeed = initial_speed + 5 |
|
self.run_cmd( |
|
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, |
|
1, # groundspeed |
|
new_target_groundspeed, |
|
-1, # throttle / no change |
|
0, # absolute values |
|
0, |
|
0, |
|
0) |
|
self.wait_groundspeed(new_target_groundspeed-0.5, new_target_groundspeed+0.5, timeout=40) |
|
self.progress("Adding some wind, ensuring groundspeed holds") |
|
self.set_parameter("SIM_WIND_SPD", 5) |
|
self.delay_sim_time(5) |
|
self.wait_groundspeed(new_target_groundspeed-0.5, new_target_groundspeed+0.5, timeout=40) |
|
self.set_parameter("SIM_WIND_SPD", 0) |
|
|
|
self.progress("Setting airspeed") |
|
new_target_airspeed = initial_speed + 5 |
|
self.run_cmd( |
|
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, |
|
0, # airspeed |
|
new_target_airspeed, |
|
-1, # throttle / no change |
|
0, # absolute values |
|
0, |
|
0, |
|
0) |
|
self.wait_groundspeed(new_target_airspeed-0.5, new_target_airspeed+0.5) |
|
self.progress("Adding some wind, hoping groundspeed increases/decreases") |
|
self.set_parameter("SIM_WIND_SPD", 5) |
|
self.set_parameter("SIM_WIND_DIR", 270) |
|
self.delay_sim_time(5) |
|
timeout = 10 |
|
tstart = self.get_sim_time() |
|
while True: |
|
if self.get_sim_time_cached() - tstart > timeout: |
|
raise NotAchievedException("Did not achieve groundspeed delta") |
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True) |
|
delta = abs(m.airspeed - m.groundspeed) |
|
want_delta = 4 |
|
self.progress("groundspeed and airspeed should be different (have=%f want=%f)" % (delta, want_delta)) |
|
if delta > want_delta: |
|
break |
|
self.fly_home_land_and_disarm() |
|
|
|
def fly_home_land_and_disarm(self): |
|
filename = os.path.join(testdir, "flaps.txt") |
|
self.progress("Using %s to fly home" % filename) |
|
self.load_mission(filename) |
|
self.change_mode("AUTO") |
|
self.mavproxy.send('wp set 7\n') |
|
self.mav.motors_disarmed_wait() |
|
|
|
def fly_flaps(self): |
|
"""Test flaps functionality.""" |
|
filename = os.path.join(testdir, "flaps.txt") |
|
self.context_push() |
|
ex = None |
|
try: |
|
flaps_ch = 5 |
|
servo_ch = 5 |
|
self.set_parameter("SERVO%u_FUNCTION" % servo_ch, 3) # flapsauto |
|
self.set_parameter("FLAP_IN_CHANNEL", flaps_ch) |
|
self.set_parameter("LAND_FLAP_PERCNT", 50) |
|
self.set_parameter("LOG_DISARMED", 1) |
|
flaps_ch_min = 1000 |
|
flaps_ch_trim = 1500 |
|
flaps_ch_max = 2000 |
|
self.set_parameter("RC%u_MIN" % flaps_ch, flaps_ch_min) |
|
self.set_parameter("RC%u_MAX" % flaps_ch, flaps_ch_max) |
|
self.set_parameter("RC%u_TRIM" % flaps_ch, flaps_ch_trim) |
|
|
|
servo_ch_min = 1200 |
|
servo_ch_trim = 1300 |
|
servo_ch_max = 1800 |
|
self.set_parameter("SERVO%u_MIN" % servo_ch, servo_ch_min) |
|
self.set_parameter("SERVO%u_MAX" % servo_ch, servo_ch_max) |
|
self.set_parameter("SERVO%u_TRIM" % servo_ch, servo_ch_trim) |
|
|
|
self.progress("check flaps are not deployed") |
|
self.set_rc(flaps_ch, flaps_ch_min) |
|
self.wait_servo_channel_value(servo_ch, servo_ch_min) |
|
self.progress("deploy the flaps") |
|
self.set_rc(flaps_ch, flaps_ch_max) |
|
tstart = self.get_sim_time() |
|
self.wait_servo_channel_value(servo_ch, servo_ch_max) |
|
tstop = self.get_sim_time_cached() |
|
delta_time = tstop - tstart |
|
delta_time_min = 0.5 |
|
delta_time_max = 1.5 |
|
if delta_time < delta_time_min or delta_time > delta_time_max: |
|
raise NotAchievedException(( |
|
"Flaps Slew not working (%f seconds)" % (delta_time,))) |
|
self.progress("undeploy flaps") |
|
self.set_rc(flaps_ch, flaps_ch_min) |
|
self.wait_servo_channel_value(servo_ch, servo_ch_min) |
|
|
|
self.progress("Flying mission %s" % filename) |
|
self.load_mission(filename) |
|
self.mavproxy.send('wp set 1\n') |
|
self.mavproxy.send('switch 1\n') # auto mode |
|
self.wait_mode('AUTO') |
|
self.wait_ready_to_arm() |
|
self.arm_vehicle() |
|
last_mission_current_msg = 0 |
|
last_seq = None |
|
while self.armed(): |
|
m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True) |
|
time_delta = (self.get_sim_time_cached() - |
|
last_mission_current_msg) |
|
if (time_delta > 1 or m.seq != last_seq): |
|
dist = None |
|
x = self.mav.messages.get("NAV_CONTROLLER_OUTPUT", None) |
|
if x is not None: |
|
dist = x.wp_dist |
|
self.progress("MISSION_CURRENT.seq=%u (dist=%s)" % |
|
(m.seq, str(dist))) |
|
last_mission_current_msg = self.get_sim_time_cached() |
|
last_seq = m.seq |
|
# flaps should undeploy at the end |
|
self.wait_servo_channel_value(servo_ch, servo_ch_min, timeout=30) |
|
|
|
# do a short flight in FBWA, watching for flaps |
|
# self.mavproxy.send('switch 4\n') |
|
# self.wait_mode('FBWA') |
|
# self.wait_seconds(10) |
|
# self.mavproxy.send('switch 6\n') |
|
# self.wait_mode('MANUAL') |
|
# self.wait_seconds(10) |
|
|
|
self.progress("Flaps OK") |
|
except Exception as e: |
|
ex = e |
|
self.context_pop() |
|
if ex: |
|
if self.armed(): |
|
self.disarm_vehicle() |
|
raise ex |
|
|
|
def test_rc_relay(self): |
|
'''test toggling channel 12 toggles relay''' |
|
self.set_parameter("RC12_OPTION", 28) # Relay On/Off |
|
self.set_rc(12, 1000) |
|
self.reboot_sitl() # needed for RC12_OPTION to take effect |
|
|
|
off = self.get_parameter("SIM_PIN_MASK") |
|
if off: |
|
raise PreconditionFailedException("SIM_MASK_PIN off") |
|
|
|
# allow time for the RC library to register initial value: |
|
self.delay_sim_time(1) |
|
|
|
self.set_rc(12, 2000) |
|
self.wait_heartbeat() |
|
self.wait_heartbeat() |
|
|
|
on = self.get_parameter("SIM_PIN_MASK") |
|
if not on: |
|
raise NotAchievedException("SIM_PIN_MASK doesn't reflect ON") |
|
self.set_rc(12, 1000) |
|
self.wait_heartbeat() |
|
self.wait_heartbeat() |
|
off = self.get_parameter("SIM_PIN_MASK") |
|
if off: |
|
raise NotAchievedException("SIM_PIN_MASK doesn't reflect OFF") |
|
|
|
def test_rc_option_camera_trigger(self): |
|
'''test toggling channel 12 takes picture''' |
|
self.set_parameter("RC12_OPTION", 9) # CameraTrigger |
|
self.reboot_sitl() # needed for RC12_OPTION to take effect |
|
|
|
x = self.mav.messages.get("CAMERA_FEEDBACK", None) |
|
if x is not None: |
|
raise PreconditionFailedException("Receiving CAMERA_FEEDBACK?!") |
|
self.set_rc(12, 2000) |
|
tstart = self.get_sim_time() |
|
while self.get_sim_time_cached() - tstart < 10: |
|
x = self.mav.messages.get("CAMERA_FEEDBACK", None) |
|
if x is not None: |
|
break |
|
self.wait_heartbeat() |
|
self.set_rc(12, 1000) |
|
if x is None: |
|
raise NotAchievedException("No CAMERA_FEEDBACK message received") |
|
|
|
def test_throttle_failsafe(self): |
|
self.change_mode('MANUAL') |
|
m = self.mav.recv_match(type='SYS_STATUS', blocking=True) |
|
receiver_bit = mavutil.mavlink.MAV_SYS_STATUS_SENSOR_RC_RECEIVER |
|
self.progress("Testing receiver enabled") |
|
if (not (m.onboard_control_sensors_enabled & receiver_bit)): |
|
raise PreconditionFailedException() |
|
self.progress("Testing receiver present") |
|
if (not (m.onboard_control_sensors_present & receiver_bit)): |
|
raise PreconditionFailedException() |
|
self.progress("Testing receiver health") |
|
if (not (m.onboard_control_sensors_health & receiver_bit)): |
|
raise PreconditionFailedException() |
|
|
|
self.progress("Ensure we know original throttle value") |
|
self.wait_rc_channel_value(3, 1000) |
|
|
|
self.set_parameter("THR_FS_VALUE", 960) |
|
self.progress("Failing receiver (throttle-to-950)") |
|
self.set_parameter("SIM_RC_FAIL", 2) # throttle-to-950 |
|
self.wait_mode('CIRCLE') # short failsafe |
|
self.wait_mode('RTL') # long failsafe |
|
self.progress("Ensure we've had our throttle squashed to 950") |
|
self.wait_rc_channel_value(3, 950) |
|
self.drain_mav_unparsed() |
|
m = self.mav.recv_match(type='SYS_STATUS', blocking=True) |
|
print("%s" % str(m)) |
|
self.progress("Testing receiver enabled") |
|
if (not (m.onboard_control_sensors_enabled & receiver_bit)): |
|
raise NotAchievedException("Receiver not enabled") |
|
self.progress("Testing receiver present") |
|
if (not (m.onboard_control_sensors_present & receiver_bit)): |
|
raise NotAchievedException("Receiver not present") |
|
# skip this until RC is fixed |
|
# self.progress("Testing receiver health") |
|
# if (m.onboard_control_sensors_health & receiver_bit): |
|
# raise NotAchievedException("Sensor healthy when it shouldn't be") |
|
self.set_parameter("SIM_RC_FAIL", 0) |
|
self.drain_mav_unparsed() |
|
# have to allow time for RC to be fetched from SITL |
|
self.delay_sim_time(0.5) |
|
m = self.mav.recv_match(type='SYS_STATUS', blocking=True) |
|
self.progress("Testing receiver enabled") |
|
if (not (m.onboard_control_sensors_enabled & receiver_bit)): |
|
raise NotAchievedException("Receiver not enabled") |
|
self.progress("Testing receiver present") |
|
if (not (m.onboard_control_sensors_present & receiver_bit)): |
|
raise NotAchievedException("Receiver not present") |
|
self.progress("Testing receiver health") |
|
if (not (m.onboard_control_sensors_health & receiver_bit)): |
|
raise NotAchievedException("Receiver not healthy2") |
|
self.change_mode('MANUAL') |
|
|
|
self.progress("Failing receiver (no-pulses)") |
|
self.set_parameter("SIM_RC_FAIL", 1) # no-pulses |
|
self.wait_mode('CIRCLE') # short failsafe |
|
self.wait_mode('RTL') # long failsafe |
|
self.drain_mav_unparsed() |
|
m = self.mav.recv_match(type='SYS_STATUS', blocking=True) |
|
print("%s" % str(m)) |
|
self.progress("Testing receiver enabled") |
|
if (not (m.onboard_control_sensors_enabled & receiver_bit)): |
|
raise NotAchievedException("Receiver not enabled") |
|
self.progress("Testing receiver present") |
|
if (not (m.onboard_control_sensors_present & receiver_bit)): |
|
raise NotAchievedException("Receiver not present") |
|
self.progress("Testing receiver health") |
|
if (m.onboard_control_sensors_health & receiver_bit): |
|
raise NotAchievedException("Sensor healthy when it shouldn't be") |
|
self.progress("Making RC work again") |
|
self.set_parameter("SIM_RC_FAIL", 0) |
|
# have to allow time for RC to be fetched from SITL |
|
self.progress("Giving receiver time to recover") |
|
self.delay_sim_time(0.5) |
|
self.drain_mav_unparsed() |
|
m = self.mav.recv_match(type='SYS_STATUS', blocking=True) |
|
self.progress("Testing receiver enabled") |
|
if (not (m.onboard_control_sensors_enabled & receiver_bit)): |
|
raise NotAchievedException("Receiver not enabled") |
|
self.progress("Testing receiver present") |
|
if (not (m.onboard_control_sensors_present & receiver_bit)): |
|
raise NotAchievedException("Receiver not present") |
|
self.progress("Testing receiver health") |
|
if (not (m.onboard_control_sensors_health & receiver_bit)): |
|
raise NotAchievedException("Receiver not healthy") |
|
self.change_mode('MANUAL') |
|
|
|
self.progress("Ensure long failsafe can trigger when short failsafe disabled") |
|
self.context_push() |
|
ex = None |
|
try: |
|
self.set_parameter("FS_SHORT_ACTN", 3) # 3 means disabled |
|
self.set_parameter("SIM_RC_FAIL", 1) |
|
self.wait_statustext("Long event on") |
|
self.wait_mode("RTL") |
|
self.set_parameter("SIM_RC_FAIL", 0) |
|
self.wait_text("Long event off") |
|
self.change_mode("MANUAL") |
|
|
|
self.progress("Trying again with THR_FS_VALUE") |
|
self.set_parameter("THR_FS_VALUE", 960) |
|
self.set_parameter("SIM_RC_FAIL", 2) |
|
self.wait_statustext("Long event on") |
|
self.wait_mode("RTL") |
|
except Exception as e: |
|
self.progress("Exception caught:") |
|
self.progress(self.get_exception_stacktrace(e)) |
|
ex = e |
|
self.context_pop() |
|
if ex is not None: |
|
raise ex |
|
|
|
def test_gripper_mission(self): |
|
self.context_push() |
|
ex = None |
|
try: |
|
self.load_mission("plane-gripper-mission.txt") |
|
self.mavproxy.send("wp set 1\n") |
|
self.change_mode('AUTO') |
|
self.wait_ready_to_arm() |
|
self.arm_vehicle() |
|
self.mavproxy.expect("Gripper Grabbed") |
|
self.mavproxy.expect("Gripper Released") |
|
self.mavproxy.expect("Auto disarmed") |
|
except Exception as e: |
|
self.progress("Exception caught:") |
|
self.progress(self.get_exception_stacktrace(e)) |
|
ex = e |
|
self.context_pop() |
|
if ex is not None: |
|
raise ex |
|
|
|
def assert_fence_sys_status(self, present, enabled, health): |
|
self.delay_sim_time(1) |
|
self.drain_mav_unparsed() |
|
m = self.mav.recv_match(type='SYS_STATUS', blocking=True, timeout=1) |
|
if m is None: |
|
raise NotAchievedException("Did not receive SYS_STATUS") |
|
tests = [ ( "present", present, m.onboard_control_sensors_present ), |
|
( "enabled", enabled, m.onboard_control_sensors_enabled ), |
|
( "health", health, m.onboard_control_sensors_health ), |
|
] |
|
bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE |
|
for test in tests: |
|
(name, want, field) = test |
|
got = (field & bit) != 0 |
|
if want != got: |
|
raise NotAchievedException("fence status incorrect; %s want=%u got=%u" % |
|
(name, want, got)) |
|
|
|
def do_fence_en_or_dis_able(self, value, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): |
|
if value: |
|
p1 = 1 |
|
else: |
|
p1 = 0 |
|
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, |
|
p1, # param1 |
|
0, # param2 |
|
0, # param3 |
|
0, # param4 |
|
0, # param5 |
|
0, # param6 |
|
0, # param7 |
|
want_result=want_result) |
|
|
|
def do_fence_enable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): |
|
self.do_fence_en_or_dis_able(True, want_result=want_result) |
|
|
|
def do_fence_disable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): |
|
self.do_fence_en_or_dis_able(False, want_result=want_result) |
|
|
|
def wait_circling_point_with_radius(self, loc, want_radius, epsilon=5.0, min_circle_time=5, timeout=120): |
|
on_radius_start_heading = None |
|
average_radius = 0.0 |
|
circle_time_start = 0 |
|
done_time = False |
|
done_angle = False |
|
tstart = self.get_sim_time() |
|
while True: |
|
if self.get_sim_time() - tstart > timeout: |
|
raise AutoTestTimeoutException("Did not get onto circle") |
|
here = self.mav.location() |
|
got_radius = self.get_distance(loc, here) |
|
average_radius = 0.95*average_radius + 0.05*got_radius |
|
on_radius = abs(got_radius - want_radius) < epsilon |
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True) |
|
heading = m.heading |
|
on_string = "off" |
|
got_angle = "" |
|
if on_radius_start_heading is not None: |
|
got_angle = "%0.2f" % abs(on_radius_start_heading - heading) # FIXME |
|
on_string = "on" |
|
|
|
want_angle = 180 # we don't actually get this (angle-substraction issue. But we get enough... |
|
self.progress("wait-circling: got-r=%0.2f want-r=%f avg-r=%f %s want-a=%0.1f got-a=%s" % |
|
(got_radius, want_radius, average_radius, on_string, want_angle, got_angle)) |
|
if on_radius: |
|
if on_radius_start_heading is None: |
|
on_radius_start_heading = heading |
|
average_radius = got_radius |
|
circle_time_start = self.get_sim_time() |
|
continue |
|
if abs(on_radius_start_heading - heading) > want_angle: # FIXME |
|
done_angle = True |
|
if self.get_sim_time() - circle_time_start > min_circle_time: |
|
done_time = True |
|
if done_time and done_angle: |
|
return |
|
continue |
|
if on_radius_start_heading is not None: |
|
average_radius = 0.0 |
|
on_radius_start_heading = None |
|
circle_time_start = 0 |
|
|
|
def test_fence_static(self): |
|
ex = None |
|
try: |
|
self.progress("Checking for bizarre healthy-when-not-present-or-enabled") |
|
self.assert_fence_sys_status(False, False, True) |
|
self.load_fence("CMAC-fence.txt") |
|
m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) |
|
if m is not None: |
|
raise NotAchievedException("Got FENCE_STATUS unexpectedly"); |
|
self.drain_mav_unparsed() |
|
self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_NONE) # report only |
|
self.assert_fence_sys_status(False, False, True) |
|
self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_RTL) # report only |
|
self.assert_fence_sys_status(True, False, True) |
|
self.mavproxy.send('fence enable\n') |
|
self.mavproxy.expect("fence enabled") |
|
self.assert_fence_sys_status(True, True, True) |
|
m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) |
|
if m is None: |
|
raise NotAchievedException("Did not get FENCE_STATUS"); |
|
if m.breach_status: |
|
raise NotAchievedException("Breached fence unexpectedly (%u)" % |
|
(m.breach_status)) |
|
self.mavproxy.send('fence disable\n') |
|
self.mavproxy.expect("fence disabled") |
|
self.assert_fence_sys_status(True, False, True) |
|
self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_NONE) |
|
self.assert_fence_sys_status(False, False, True) |
|
self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_RTL) |
|
self.assert_fence_sys_status(True, False, True) |
|
self.mavproxy.send("fence clear\n") |
|
self.mavproxy.expect("fence removed") |
|
if self.get_parameter("FENCE_TOTAL") != 0: |
|
raise NotAchievedException("Expected zero points remaining") |
|
self.assert_fence_sys_status(False, False, True) |
|
self.progress("Trying to enable fence with no points") |
|
self.do_fence_enable(want_result=mavutil.mavlink.MAV_RESULT_FAILED) |
|
|
|
# test a rather unfortunate behaviour: |
|
self.progress("Killing a live fence with fence-clear") |
|
self.load_fence("CMAC-fence.txt") |
|
self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_RTL) |
|
self.do_fence_enable() |
|
self.assert_fence_sys_status(True, True, True) |
|
self.mavproxy.send("fence clear\n") |
|
self.mavproxy.expect("fence removed") |
|
if self.get_parameter("FENCE_TOTAL") != 0: |
|
raise NotAchievedException("Expected zero points remaining") |
|
self.assert_fence_sys_status(False, False, True) |
|
|
|
except Exception as e: |
|
self.progress("Exception caught:") |
|
self.progress(self.get_exception_stacktrace(e)) |
|
ex = e |
|
self.mavproxy.send('fence clear\n') |
|
if ex is not None: |
|
raise ex |
|
|
|
def test_fence_breach_circle_at(self, loc, disable_on_breach=False): |
|
ex = None |
|
try: |
|
self.load_fence("CMAC-fence.txt") |
|
want_radius = 100 |
|
# when ArduPlane is fixed, remove this fudge factor |
|
REALLY_BAD_FUDGE_FACTOR = 1.16 |
|
expected_radius = REALLY_BAD_FUDGE_FACTOR * want_radius |
|
self.set_parameter("RTL_RADIUS", want_radius) |
|
self.set_parameter("NAVL1_LIM_BANK", 60) |
|
self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_RTL) |
|
|
|
self.do_fence_enable() |
|
self.assert_fence_sys_status(True, True, True) |
|
|
|
self.takeoff(alt=45, alt_max=300) |
|
|
|
tstart = self.get_sim_time() |
|
while True: |
|
if self.get_sim_time() - tstart > 30: |
|
raise NotAchievedException("Did not breach fence") |
|
m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) |
|
if m is None: |
|
raise NotAchievedException("Did not get FENCE_STATUS"); |
|
if m.breach_status == 0: |
|
continue |
|
|
|
# we've breached; check our state; |
|
if m.breach_type != mavutil.mavlink.FENCE_BREACH_BOUNDARY: |
|
raise NotAchievedException("Unexpected breach type %u" % |
|
(m.breach_type,)) |
|
if m.breach_count == 0: |
|
raise NotAchievedException("Unexpected breach count %u" % |
|
(m.breach_count,)) |
|
self.assert_fence_sys_status(True, True, False) |
|
break |
|
|
|
if disable_on_breach: |
|
self.do_fence_disable() |
|
|
|
self.wait_circling_point_with_radius(loc, expected_radius) |
|
|
|
self.disarm_vehicle(force=True) |
|
self.reboot_sitl() |
|
|
|
except Exception as e: |
|
self.progress("Exception caught:") |
|
self.progress(self.get_exception_stacktrace(e)) |
|
ex = e |
|
self.mavproxy.send('fence clear\n') |
|
if ex is not None: |
|
raise ex |
|
|
|
def test_fence_rtl(self): |
|
self.progress("Testing FENCE_ACTION_RTL no rally point") |
|
# have to disable the fence once we've breached or we breach |
|
# it as part of the loiter-at-home! |
|
self.test_fence_breach_circle_at(self.home_position_as_mav_location(), |
|
disable_on_breach=True) |
|
|
|
def test_fence_rtl_rally(self): |
|
ex = None |
|
target_system = 1 |
|
target_component = 1 |
|
try: |
|
self.progress("Testing FENCE_ACTION_RTL with rally point") |
|
|
|
self.wait_ready_to_arm() |
|
loc = self.home_position_as_mav_location() |
|
self.location_offset_ne(loc, 50, -50) |
|
|
|
self.set_parameter("RALLY_TOTAL", 1) |
|
self.mav.mav.rally_point_send(target_system, |
|
target_component, |
|
0, # sequence number |
|
1, # total count |
|
int(loc.lat * 1e7), |
|
int(loc.lng * 1e7), |
|
15, |
|
0, # "break" alt?! |
|
0, # "land dir" |
|
0) # flags |
|
self.delay_sim_time(1) |
|
self.mavproxy.send("rally list\n") |
|
self.test_fence_breach_circle_at(loc) |
|
except Exception as e: |
|
self.progress("Exception caught:") |
|
self.progress(self.get_exception_stacktrace(e)) |
|
ex = e |
|
self.mavproxy.send('rally clear\n') |
|
if ex is not None: |
|
raise ex |
|
|
|
def test_parachute(self): |
|
self.set_rc(9, 1000) |
|
self.set_parameter("CHUTE_ENABLED", 1) |
|
self.set_parameter("CHUTE_TYPE", 10) |
|
self.set_parameter("SERVO9_FUNCTION", 27) |
|
self.set_parameter("SIM_PARA_ENABLE", 1) |
|
self.set_parameter("SIM_PARA_PIN", 9) |
|
|
|
self.load_mission("plane-parachute-mission.txt") |
|
self.mavproxy.send("wp set 1\n") |
|
self.change_mode('AUTO') |
|
self.wait_ready_to_arm() |
|
self.arm_vehicle() |
|
self.mavproxy.expect("BANG") |
|
self.disarm_vehicle(force=True) |
|
self.reboot_sitl() |
|
|
|
def test_parachute_sinkrate(self): |
|
self.set_rc(9, 1000) |
|
self.set_parameter("CHUTE_ENABLED", 1) |
|
self.set_parameter("CHUTE_TYPE", 10) |
|
self.set_parameter("SERVO9_FUNCTION", 27) |
|
self.set_parameter("SIM_PARA_ENABLE", 1) |
|
self.set_parameter("SIM_PARA_PIN", 9) |
|
|
|
self.set_parameter("CHUTE_CRT_SINK", 9) |
|
|
|
self.progress("Takeoff") |
|
self.takeoff(alt=300) |
|
|
|
self.progress("Diving") |
|
self.set_rc(2, 2000) |
|
self.mavproxy.expect("BANG") |
|
|
|
self.disarm_vehicle(force=True) |
|
self.reboot_sitl() |
|
|
|
def run_subtest(self, desc, func): |
|
self.start_subtest(desc) |
|
func() |
|
|
|
def test_main_flight(self): |
|
|
|
self.change_mode('MANUAL') |
|
|
|
# grab home position: |
|
self.mav.recv_match(type='HOME_POSITION', blocking=True) |
|
self.homeloc = self.mav.location() |
|
|
|
self.run_subtest("Takeoff", self.takeoff) |
|
|
|
self.run_subtest("Set Attitude Target", self.set_attitude_target) |
|
|
|
self.run_subtest("Fly left circuit", self.fly_left_circuit) |
|
|
|
self.run_subtest("Left roll", lambda: self.axial_left_roll(1)) |
|
|
|
self.run_subtest("Inside loop", self.inside_loop) |
|
|
|
self.run_subtest("Stablize test", self.test_stabilize) |
|
|
|
self.run_subtest("ACRO test", self.test_acro) |
|
|
|
self.run_subtest("FBWB test", self.test_FBWB) |
|
|
|
self.run_subtest("CRUISE test", lambda: self.test_FBWB(mode='CRUISE')) |
|
|
|
self.run_subtest("RTL test", self.fly_RTL) |
|
|
|
self.run_subtest("LOITER test", self.fly_LOITER) |
|
|
|
self.run_subtest("CIRCLE test", self.fly_CIRCLE) |
|
|
|
self.run_subtest("Mission test", |
|
lambda: self.fly_mission( |
|
os.path.join(testdir, "ap1.txt"))) |
|
|
|
def airspeed_autocal(self): |
|
self.progress("Ensure no AIRSPEED_AUTOCAL on ground") |
|
self.set_parameter("ARSPD_AUTOCAL", 1) |
|
m = self.mav.recv_match(type='AIRSPEED_AUTOCAL', |
|
blocking=True, |
|
timeout=5) |
|
if m is not None: |
|
raise NotAchievedException("Got autocal on ground") |
|
mission_filepath = os.path.join(testdir, "flaps.txt") |
|
self.load_mission(mission_filepath) |
|
self.wait_ready_to_arm() |
|
self.arm_vehicle() |
|
self.change_mode("AUTO") |
|
self.progress("Ensure AIRSPEED_AUTOCAL in air") |
|
m = self.mav.recv_match(type='AIRSPEED_AUTOCAL', |
|
blocking=True, |
|
timeout=5) |
|
self.mav.motors_disarmed_wait() |
|
|
|
def test_rangefinder(self): |
|
ex = None |
|
self.context_push() |
|
self.progress("Making sure we don't ordinarily get RANGEFINDER") |
|
m = None |
|
try: |
|
m = self.mav.recv_match(type='RANGEFINDER', |
|
blocking=True, |
|
timeout=5) |
|
except Exception as e: |
|
self.progress("Caught exception: %s" % |
|
self.get_exception_stacktrace(e)) |
|
|
|
if m is not None: |
|
raise NotAchievedException("Received unexpected RANGEFINDER msg") |
|
|
|
try: |
|
self.set_analog_rangefinder_parameters() |
|
|
|
self.reboot_sitl() |
|
|
|
'''ensure rangefinder gives height-above-ground''' |
|
self.load_mission("plane-gripper-mission.txt") # borrow this |
|
self.mavproxy.send("wp set 1\n") |
|
self.change_mode('AUTO') |
|
self.wait_ready_to_arm() |
|
self.arm_vehicle() |
|
self.wait_waypoint(5, 5, max_dist=100) |
|
rf = self.mav.recv_match(type="RANGEFINDER", timeout=1, blocking=True) |
|
if rf is None: |
|
raise NotAchievedException("Did not receive rangefinder message") |
|
gpi = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1) |
|
if gpi is None: |
|
raise NotAchievedException("Did not receive GLOBAL_POSITION_INT message") |
|
if abs(rf.distance - gpi.relative_alt/1000.0) > 3: |
|
raise NotAchievedException("rangefinder alt (%s) disagrees with global-position-int.relative_alt (%s)" % (rf.distance, gpi.relative_alt/1000.0)) |
|
self.mavproxy.expect("Auto disarmed") |
|
|
|
self.progress("Ensure RFND messages in log") |
|
if not self.current_onboard_log_contains_message("RFND"): |
|
raise NotAchievedException("No RFND messages in log") |
|
|
|
except Exception as e: |
|
self.progress("Exception caught:") |
|
self.progress(self.get_exception_stacktrace(e)) |
|
ex = e |
|
self.context_pop() |
|
self.reboot_sitl() |
|
if ex is not None: |
|
raise ex |
|
|
|
def rc_defaults(self): |
|
ret = super(AutoTestPlane, self).rc_defaults() |
|
ret[3] = 1000 |
|
ret[8] = 1800 |
|
return ret |
|
|
|
def default_mode(self): |
|
return "MANUAL" |
|
|
|
def test_pid_tuning(self): |
|
self.change_mode("FBWA") # we don't update PIDs in MANUAL |
|
super(AutoTestPlane, self).test_pid_tuning() |
|
|
|
|
|
def test_setting_modes_via_auxswitches(self): |
|
self.set_parameter("FLTMODE5", 1) |
|
self.mavproxy.send('switch 1\n') # random mode |
|
self.wait_heartbeat() |
|
self.change_mode('MANUAL') |
|
self.mavproxy.send('switch 5\n') # acro mode |
|
self.wait_mode("CIRCLE") |
|
self.set_rc(9, 1000) |
|
self.set_rc(10, 1000) |
|
self.set_parameter("RC9_OPTION", 4) # RTL |
|
self.set_parameter("RC10_OPTION", 55) # guided |
|
self.set_rc(9, 1900) |
|
self.wait_mode("RTL") |
|
self.set_rc(10, 1900) |
|
self.wait_mode("GUIDED") |
|
|
|
self.progress("resetting both switches - should go back to CIRCLE") |
|
self.set_rc(9, 1000) |
|
self.set_rc(10, 1000) |
|
self.wait_mode("CIRCLE") |
|
|
|
self.set_rc(9, 1900) |
|
self.wait_mode("RTL") |
|
self.set_rc(10, 1900) |
|
self.wait_mode("GUIDED") |
|
|
|
self.progress("Resetting switch should repoll mode switch") |
|
self.set_rc(10, 1000) # this re-polls the mode switch |
|
self.wait_mode("CIRCLE") |
|
self.set_rc(9, 1000) |
|
|
|
def test_adsb(self): |
|
self.wait_ready_to_arm() |
|
here = self.mav.location() |
|
# message ADSB_VEHICLE 37 -353632614 1491652305 0 584070 0 0 0 "bob" 3 1 255 17 |
|
self.set_parameter("ADSB_ENABLE", 1) |
|
self.set_parameter("AVD_ENABLE", 1) |
|
self.delay_sim_time(2) # TODO: work out why this is required... |
|
self.mav.mav.adsb_vehicle_send(37, # ICAO address |
|
int(here.lat * 1e7), |
|
int(here.lng * 1e7), |
|
mavutil.mavlink.ADSB_ALTITUDE_TYPE_PRESSURE_QNH, |
|
int(here.alt*1000 + 10000), # 10m up |
|
0, # heading in cdeg |
|
0, # horizontal velocity cm/s |
|
0, # vertical velocity cm/s |
|
"bob".encode("ascii"), # callsign |
|
mavutil.mavlink.ADSB_EMITTER_TYPE_LIGHT, |
|
1, # time since last communication |
|
65535, # flags |
|
17 # squawk |
|
) |
|
self.progress("Waiting for collision message") |
|
m = self.mav.recv_match(type='COLLISION', blocking=True, timeout=4) |
|
if m is None: |
|
raise NotAchievedException("Did not get collision message") |
|
if m.threat_level != 2: |
|
raise NotAchievedException("Expected some threat at least") |
|
|
|
def tests(self): |
|
'''return list of all tests''' |
|
ret = super(AutoTestPlane, self).tests() |
|
ret.extend([ |
|
|
|
("AuxModeSwitch", |
|
"Set modes via auxswitches", |
|
self.test_setting_modes_via_auxswitches), |
|
|
|
("TestRCCamera", |
|
"Test RC Option - Camera Trigger", |
|
self.test_rc_option_camera_trigger), |
|
|
|
("TestRCRelay", "Test Relay RC Channel Option", self.test_rc_relay), |
|
|
|
("ThrottleFailsafe", |
|
"Fly throttle failsafe", |
|
self.test_throttle_failsafe), |
|
|
|
("TestFlaps", "Flaps", self.fly_flaps), |
|
|
|
("DO_CHANGE_SPEED", "Test mavlink DO_CHANGE_SPEED command", self.fly_do_change_speed), |
|
|
|
("DO_REPOSITION", |
|
"Test mavlink DO_REPOSITION command", |
|
self.fly_do_reposition), |
|
|
|
("MainFlight", |
|
"Lots of things in one flight", |
|
self.test_main_flight), |
|
|
|
("TestGripperMission", |
|
"Test Gripper mission items", |
|
self.test_gripper_mission), |
|
|
|
("Parachute", "Test Parachute", self.test_parachute), |
|
|
|
("ParachuteSinkRate", "Test Parachute (SinkRate triggering)", self.test_parachute_sinkrate), |
|
|
|
("AIRSPEED_AUTOCAL", "Test AIRSPEED_AUTOCAL", self.airspeed_autocal), |
|
|
|
("RangeFinder", |
|
"Test RangeFinder Basic Functionality", |
|
self.test_rangefinder), |
|
|
|
("FenceStatic", |
|
"Test Basic Fence Functionality", |
|
self.test_fence_static), |
|
|
|
("FenceRTL", |
|
"Test Fence RTL", |
|
self.test_fence_rtl), |
|
|
|
("FenceRTLRally", |
|
"Test Fence RTL Rally", |
|
self.test_fence_rtl_rally), |
|
|
|
("ADSB", |
|
"Test ADSB", |
|
self.test_adsb), |
|
|
|
("AdvancedFailsafe", |
|
"Test Advanced Failsafe", |
|
self.test_advanced_failsafe), |
|
|
|
("DeepStall", |
|
"Test DeepStall Landing", |
|
self.fly_deepstall), |
|
|
|
("LogDownLoad", |
|
"Log download", |
|
lambda: self.log_download( |
|
self.buildlogs_path("ArduPlane-log.bin"), |
|
timeout=450, |
|
upload_logs=True)) |
|
]) |
|
return ret
|
|
|