mirror of https://github.com/ArduPilot/ardupilot
autotest: tidy various tests to take account of new framework capabilities
autotest: simplify AutoDock test autotest: tidy AP_Proximity_MAV test autotest: tidy DriveSquare test autotest: tidy BatteryFailsafe test autotest: tidy GPSViconSwitching test autotest: tidy RangeFinder test
This commit is contained in:
Peter Barker
Peter Barker
parent
478d26d69c
commit
bedc76e0e8
|
|
@ -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'''
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
Loading…
Reference in New Issue