diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index f41d11fbe7..17b5003675 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -961,21 +961,6 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): # Tests all actions and logic behind the battery failsafe def BatteryFailsafe(self, timeout=300): '''Fly Battery Failsafe''' - self.context_push() - ex = None - try: - self.test_battery_failsafe(timeout=timeout) - except Exception as e: - self.print_exception_caught(e) - self.disarm_vehicle(force=True) - ex = e - self.context_pop() - self.reboot_sitl() - - if ex is not None: - raise ex - - def test_battery_failsafe(self, timeout=300): self.progress("Configure battery failsafe parameters") self.set_parameters({ 'SIM_SPEEDUP': 4, @@ -3730,82 +3715,68 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): def GPSViconSwitching(self): """Fly GPS and Vicon switching test""" + """Setup parameters including switching to EKF3""" + self.set_parameters({ + "VISO_TYPE": 2, # enable vicon + "SERIAL5_PROTOCOL": 2, + "EK3_ENABLE": 1, + "EK3_SRC2_POSXY": 6, # External Nav + "EK3_SRC2_POSZ": 6, # External Nav + "EK3_SRC2_VELXY": 6, # External Nav + "EK3_SRC2_VELZ": 6, # External Nav + "EK3_SRC2_YAW": 6, # External Nav + "RC7_OPTION": 80, # RC aux switch 7 set to Viso Align + "RC8_OPTION": 90, # RC aux switch 8 set to EKF source selector + "EK2_ENABLE": 0, + "AHRS_EKF_TYPE": 3, + }) self.customise_SITL_commandline(["--serial5=sim:vicon:"]) - """Setup parameters including switching to EKF3""" - self.context_push() - ex = None - try: - self.set_parameters({ - "VISO_TYPE": 2, # enable vicon - "SERIAL5_PROTOCOL": 2, - "EK3_ENABLE": 1, - "EK3_SRC2_POSXY": 6, # External Nav - "EK3_SRC2_POSZ": 6, # External Nav - "EK3_SRC2_VELXY": 6, # External Nav - "EK3_SRC2_VELZ": 6, # External Nav - "EK3_SRC2_YAW": 6, # External Nav - "RC7_OPTION": 80, # RC aux switch 7 set to Viso Align - "RC8_OPTION": 90, # RC aux switch 8 set to EKF source selector - "EK2_ENABLE": 0, - "AHRS_EKF_TYPE": 3, - }) - self.reboot_sitl() + # switch to use GPS + self.set_rc(8, 1000) - # switch to use GPS - self.set_rc(8, 1000) + # ensure we can get a global position: + self.poll_home_position(timeout=120) - # ensure we can get a global position: - self.poll_home_position(timeout=120) + # record starting position + old_pos = self.get_global_position_int() + print("old_pos=%s" % str(old_pos)) - # record starting position - old_pos = self.get_global_position_int() - print("old_pos=%s" % str(old_pos)) + # align vicon yaw with ahrs heading + self.set_rc(7, 2000) - # align vicon yaw with ahrs heading - self.set_rc(7, 2000) + # takeoff to 10m in Loiter + self.progress("Moving to ensure location is tracked") + self.takeoff(10, mode="LOITER", require_absolute=True, timeout=720) - # takeoff to 10m in Loiter - self.progress("Moving to ensure location is tracked") - self.takeoff(10, mode="LOITER", require_absolute=True, timeout=720) + # fly forward in Loiter + self.set_rc(2, 1300) - # fly forward in Loiter - self.set_rc(2, 1300) + # disable vicon + self.set_parameter("SIM_VICON_FAIL", 1) - # disable vicon - self.set_parameter("SIM_VICON_FAIL", 1) + # ensure vehicle remain in Loiter for 15 seconds + tstart = self.get_sim_time() + while self.get_sim_time() - tstart < 15: + if not self.mode_is('LOITER'): + raise NotAchievedException("Expected to stay in loiter for >15 seconds") - # ensure vehicle remain in Loiter for 15 seconds - tstart = self.get_sim_time() - while self.get_sim_time() - tstart < 15: - if not self.mode_is('LOITER'): - raise NotAchievedException("Expected to stay in loiter for >15 seconds") + # re-enable vicon + self.set_parameter("SIM_VICON_FAIL", 0) - # re-enable vicon - self.set_parameter("SIM_VICON_FAIL", 0) + # switch to vicon, disable GPS and wait 10sec to ensure vehicle remains in Loiter + self.set_rc(8, 1500) + self.set_parameter("GPS1_TYPE", 0) - # switch to vicon, disable GPS and wait 10sec to ensure vehicle remains in Loiter - self.set_rc(8, 1500) - self.set_parameter("GPS1_TYPE", 0) + # ensure vehicle remain in Loiter for 15 seconds + tstart = self.get_sim_time() + while self.get_sim_time() - tstart < 15: + if not self.mode_is('LOITER'): + raise NotAchievedException("Expected to stay in loiter for >15 seconds") - # ensure vehicle remain in Loiter for 15 seconds - tstart = self.get_sim_time() - while self.get_sim_time() - tstart < 15: - if not self.mode_is('LOITER'): - raise NotAchievedException("Expected to stay in loiter for >15 seconds") - - # RTL and check vehicle arrives within 10m of home - self.set_rc(2, 1500) - self.do_RTL() - - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() - self.disarm_vehicle(force=True) - self.reboot_sitl() - if ex is not None: - raise ex + # RTL and check vehicle arrives within 10m of home + self.set_rc(2, 1500) + self.do_RTL() def RTLSpeed(self): """Test RTL Speed parameters""" @@ -3885,9 +3856,6 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): def RangeFinder(self): '''Test RangeFinder Basic Functionality''' - ex = None - self.context_push() - self.progress("Making sure we don't ordinarily get RANGEFINDER") m = self.mav.recv_match(type='RANGEFINDER', blocking=True, @@ -3903,68 +3871,59 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): if self.current_onboard_log_contains_message("RFND"): raise NotAchievedException("Found unexpected RFND message") - try: - self.set_analog_rangefinder_parameters() - self.set_parameter("RC9_OPTION", 10) # rangefinder - self.set_rc(9, 2000) + self.set_analog_rangefinder_parameters() + self.set_parameter("RC9_OPTION", 10) # rangefinder + self.set_rc(9, 2000) - self.reboot_sitl() - - self.progress("Making sure we now get RANGEFINDER messages") - m = self.assert_receive_message('RANGEFINDER', timeout=10) - - self.progress("Checking RangeFinder is marked as enabled in mavlink") - m = self.mav.recv_match(type='SYS_STATUS', - blocking=True, - timeout=10) - flags = m.onboard_control_sensors_enabled - if not flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION: - raise NotAchievedException("Laser not enabled in SYS_STATUS") - self.progress("Disabling laser using switch") - self.set_rc(9, 1000) - self.delay_sim_time(1) - self.progress("Checking RangeFinder is marked as disabled in mavlink") - m = self.mav.recv_match(type='SYS_STATUS', - blocking=True, - timeout=10) - flags = m.onboard_control_sensors_enabled - if flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION: - raise NotAchievedException("Laser enabled in SYS_STATUS") - - self.progress("Re-enabling rangefinder") - self.set_rc(9, 2000) - self.delay_sim_time(1) - m = self.mav.recv_match(type='SYS_STATUS', - blocking=True, - timeout=10) - flags = m.onboard_control_sensors_enabled - if not flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION: - raise NotAchievedException("Laser not enabled in SYS_STATUS") - - self.takeoff(10, mode="LOITER") - - m_r = self.mav.recv_match(type='RANGEFINDER', - blocking=True) - m_p = self.mav.recv_match(type='GLOBAL_POSITION_INT', - blocking=True) - - if abs(m_r.distance - m_p.relative_alt/1000) > 1: - raise NotAchievedException( - "rangefinder/global position int mismatch %0.2f vs %0.2f" % - (m_r.distance, m_p.relative_alt/1000)) - - self.land_and_disarm() - - if not self.current_onboard_log_contains_message("RFND"): - raise NotAchievedException("Did not see expected RFND message") - - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() self.reboot_sitl() - if ex is not None: - raise ex + + self.progress("Making sure we now get RANGEFINDER messages") + m = self.assert_receive_message('RANGEFINDER', timeout=10) + + self.progress("Checking RangeFinder is marked as enabled in mavlink") + m = self.mav.recv_match(type='SYS_STATUS', + blocking=True, + timeout=10) + flags = m.onboard_control_sensors_enabled + if not flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION: + raise NotAchievedException("Laser not enabled in SYS_STATUS") + self.progress("Disabling laser using switch") + self.set_rc(9, 1000) + self.delay_sim_time(1) + self.progress("Checking RangeFinder is marked as disabled in mavlink") + m = self.mav.recv_match(type='SYS_STATUS', + blocking=True, + timeout=10) + flags = m.onboard_control_sensors_enabled + if flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION: + raise NotAchievedException("Laser enabled in SYS_STATUS") + + self.progress("Re-enabling rangefinder") + self.set_rc(9, 2000) + self.delay_sim_time(1) + m = self.mav.recv_match(type='SYS_STATUS', + blocking=True, + timeout=10) + flags = m.onboard_control_sensors_enabled + if not flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION: + raise NotAchievedException("Laser not enabled in SYS_STATUS") + + self.takeoff(10, mode="LOITER") + + m_r = self.mav.recv_match(type='RANGEFINDER', + blocking=True) + m_p = self.mav.recv_match(type='GLOBAL_POSITION_INT', + blocking=True) + + if abs(m_r.distance - m_p.relative_alt/1000) > 1: + raise NotAchievedException( + "rangefinder/global position int mismatch %0.2f vs %0.2f" % + (m_r.distance, m_p.relative_alt/1000)) + + self.land_and_disarm() + + if not self.current_onboard_log_contains_message("RFND"): + raise NotAchievedException("Did not see expected RFND message") def SplineTerrain(self): '''Test Splines and Terrain''' diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 941ae2f766..b11920ead0 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -86,86 +86,76 @@ class AutoTestRover(vehicle_test_suite.TestSuite): def DriveSquare(self, side=50): """Learn/Drive Square with Ch7 option""" - self.context_push() - ex = None - try: - self.progress("TEST SQUARE") - self.set_parameters({ - "RC7_OPTION": 7, - "RC9_OPTION": 58, - }) + self.progress("TEST SQUARE") + self.set_parameters({ + "RC7_OPTION": 7, + "RC9_OPTION": 58, + }) - self.change_mode('MANUAL') + self.change_mode('MANUAL') - self.wait_ready_to_arm() - self.arm_vehicle() + self.wait_ready_to_arm() + self.arm_vehicle() - self.clear_wp(9) + self.clear_wp(9) - # first aim north - self.progress("\nTurn right towards north") - self.reach_heading_manual(10) - # save bottom left corner of box as home AND waypoint - self.progress("Save HOME") - self.save_wp() + # first aim north + self.progress("\nTurn right towards north") + self.reach_heading_manual(10) + # save bottom left corner of box as home AND waypoint + self.progress("Save HOME") + self.save_wp() - self.progress("Save WP") - self.save_wp() + self.progress("Save WP") + self.save_wp() - # pitch forward to fly north - self.progress("\nGoing north %u meters" % side) - self.reach_distance_manual(side) - # save top left corner of square as waypoint - self.progress("Save WP") - self.save_wp() + # pitch forward to fly north + self.progress("\nGoing north %u meters" % side) + self.reach_distance_manual(side) + # save top left corner of square as waypoint + self.progress("Save WP") + self.save_wp() - # roll right to fly east - self.progress("\nGoing east %u meters" % side) - self.reach_heading_manual(100) - self.reach_distance_manual(side) - # save top right corner of square as waypoint - self.progress("Save WP") - self.save_wp() + # roll right to fly east + self.progress("\nGoing east %u meters" % side) + self.reach_heading_manual(100) + self.reach_distance_manual(side) + # save top right corner of square as waypoint + self.progress("Save WP") + self.save_wp() - # pitch back to fly south - self.progress("\nGoing south %u meters" % side) - self.reach_heading_manual(190) - self.reach_distance_manual(side) - # save bottom right corner of square as waypoint - self.progress("Save WP") - self.save_wp() + # pitch back to fly south + self.progress("\nGoing south %u meters" % side) + self.reach_heading_manual(190) + self.reach_distance_manual(side) + # save bottom right corner of square as waypoint + self.progress("Save WP") + self.save_wp() - # roll left to fly west - self.progress("\nGoing west %u meters" % side) - self.reach_heading_manual(280) - self.reach_distance_manual(side) - # save bottom left corner of square (should be near home) as waypoint - self.progress("Save WP") - self.save_wp() + # roll left to fly west + self.progress("\nGoing west %u meters" % side) + self.reach_heading_manual(280) + self.reach_distance_manual(side) + # save bottom left corner of square (should be near home) as waypoint + self.progress("Save WP") + self.save_wp() - self.progress("Checking number of saved waypoints") - mavproxy = self.start_mavproxy() - num_wp = self.save_mission_to_file_using_mavproxy( - mavproxy, - os.path.join(testdir, "ch7_mission.txt")) - self.stop_mavproxy(mavproxy) - expected = 7 # home + 6 toggled in - if num_wp != expected: - raise NotAchievedException("Did not get %u waypoints; got %u" % - (expected, num_wp)) + self.progress("Checking number of saved waypoints") + mavproxy = self.start_mavproxy() + num_wp = self.save_mission_to_file_using_mavproxy( + mavproxy, + os.path.join(testdir, "ch7_mission.txt")) + self.stop_mavproxy(mavproxy) + expected = 7 # home + 6 toggled in + if num_wp != expected: + raise NotAchievedException("Did not get %u waypoints; got %u" % + (expected, num_wp)) - # TODO: actually drive the mission + # TODO: actually drive the mission - self.clear_wp(9) - except Exception as e: - self.print_exception_caught(e) - ex = e + self.clear_wp(9) self.disarm_vehicle() - self.context_pop() - - if ex: - raise ex def drive_left_circuit(self): """Drive a left circuit, 50m on a side.""" @@ -5592,65 +5582,55 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) def AP_Proximity_MAV(self): '''Test MAV proximity backend''' - self.context_push() - ex = None - try: - self.set_parameters({ - "PRX1_TYPE": 2, # AP_Proximity_MAV - "OA_TYPE": 2, # dijkstra - "OA_DB_OUTPUT": 3, # send all items - }) - self.reboot_sitl() - # 1 laser pointing straight forward: - self.send_obstacle_distances_expect_distance_sensor_messages( - { - "distances": [234], - "increment_f": 10, - "angle_offset": 0.0, - "min_distance": 0, - "max_distance": 1000, # cm - }, [ - {"orientation": 0, "distance": 234}, - ]) - - # 5 lasers at front of vehicle, spread over 40 degrees: - self.send_obstacle_distances_expect_distance_sensor_messages( - { - "distances": [111, 222, 333, 444, 555], - "increment_f": 10, - "angle_offset": -20.0, - "min_distance": 0, - "max_distance": 1000, # cm - }, [ - {"orientation": 0, "distance": 111}, - ]) - - # lots of dense readings (e.g. vision camera: - distances = [0] * 72 - for i in range(0, 72): - distances[i] = 1000 + 10*abs(36-i) - - self.send_obstacle_distances_expect_distance_sensor_messages( - { - "distances": distances, - "increment_f": 90/72.0, - "angle_offset": -45.0, - "min_distance": 0, - "max_distance": 2000, # cm - }, [ - {"orientation": 0, "distance": 1000}, - {"orientation": 1, "distance": 1190}, - {"orientation": 7, "distance": 1190}, - ]) - - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() + self.set_parameters({ + "PRX1_TYPE": 2, # AP_Proximity_MAV + "OA_TYPE": 2, # dijkstra + "OA_DB_OUTPUT": 3, # send all items + }) self.reboot_sitl() - if ex is not None: - raise ex + + # 1 laser pointing straight forward: + self.send_obstacle_distances_expect_distance_sensor_messages( + { + "distances": [234], + "increment_f": 10, + "angle_offset": 0.0, + "min_distance": 0, + "max_distance": 1000, # cm + }, [ + {"orientation": 0, "distance": 234}, + ]) + + # 5 lasers at front of vehicle, spread over 40 degrees: + self.send_obstacle_distances_expect_distance_sensor_messages( + { + "distances": [111, 222, 333, 444, 555], + "increment_f": 10, + "angle_offset": -20.0, + "min_distance": 0, + "max_distance": 1000, # cm + }, [ + {"orientation": 0, "distance": 111}, + ]) + + # lots of dense readings (e.g. vision camera: + distances = [0] * 72 + for i in range(0, 72): + distances[i] = 1000 + 10*abs(36-i) + + self.send_obstacle_distances_expect_distance_sensor_messages( + { + "distances": distances, + "increment_f": 90/72.0, + "angle_offset": -45.0, + "min_distance": 0, + "max_distance": 2000, # cm + }, [ + {"orientation": 0, "distance": 1000}, + {"orientation": 1, "distance": 1190}, + {"orientation": 7, "distance": 1190}, + ]) def SendToComponents(self): '''Test ArduPilot send_to_components function''' @@ -6295,8 +6275,6 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) def AutoDock(self): '''Test automatic docking of rover for multiple FOVs of simulated beacon''' - self.context_push() - self.set_parameters({ "PLND_ENABLED": 1, "PLND_TYPE": 4, @@ -6322,40 +6300,27 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) }) for type in range(0, 3): # CYLINDRICAL FOV, CONICAL FOV, SPHERICAL FOV - ex = None - try: - self.set_parameter("SIM_PLD_TYPE", type) - self.reboot_sitl() - self.change_mode('GUIDED') - self.wait_ready_to_arm() - self.arm_vehicle() - initial_position = self.offset_location_ne(target, -20, -2) - self.drive_to_location(initial_position) - self.change_mode(8) # DOCK mode - max_delta = 1 - self.wait_distance_to_location(target, 0, max_delta, timeout=180) - self.disarm_vehicle() - self.assert_receive_message('GLOBAL_POSITION_INT') - new_pos = self.mav.location() - delta = abs(self.get_distance(target, new_pos) - stopping_dist) - self.progress("Docked %f metres from stopping point" % delta) - if delta > max_delta: - raise NotAchievedException("Did not dock close enough to stopping point (%fm > %fm" % (delta, max_delta)) + self.set_parameter("SIM_PLD_TYPE", type) + self.reboot_sitl() + self.change_mode('GUIDED') + self.wait_ready_to_arm() + self.arm_vehicle() + initial_position = self.offset_location_ne(target, -20, -2) + self.drive_to_location(initial_position) + self.change_mode(8) # DOCK mode + max_delta = 1 + self.wait_distance_to_location(target, 0, max_delta, timeout=180) + self.disarm_vehicle() + self.assert_receive_message('GLOBAL_POSITION_INT') + new_pos = self.mav.location() + delta = abs(self.get_distance(target, new_pos) - stopping_dist) + self.progress("Docked %f metres from stopping point" % delta) + if delta > max_delta: + raise NotAchievedException("Did not dock close enough to stopping point (%fm > %fm" % (delta, max_delta)) - if not self.current_onboard_log_contains_message("PL"): - raise NotAchievedException("Did not see expected PL message") + if not self.current_onboard_log_contains_message("PL"): + raise NotAchievedException("Did not see expected PL message") - except Exception as e: - self.print_exception_caught(e) - ex = e - break - - self.context_pop() - - if ex is not None: - raise ex - - self.reboot_sitl() self.progress("All done") def PrivateChannel(self):