#!/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