mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
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
|
# Tests all actions and logic behind the battery failsafe
|
||||||
def BatteryFailsafe(self, timeout=300):
|
def BatteryFailsafe(self, timeout=300):
|
||||||
'''Fly Battery Failsafe'''
|
'''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.progress("Configure battery failsafe parameters")
|
||||||
self.set_parameters({
|
self.set_parameters({
|
||||||
'SIM_SPEEDUP': 4,
|
'SIM_SPEEDUP': 4,
|
||||||
@ -3730,82 +3715,68 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
|
|||||||
|
|
||||||
def GPSViconSwitching(self):
|
def GPSViconSwitching(self):
|
||||||
"""Fly GPS and Vicon switching test"""
|
"""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:"])
|
self.customise_SITL_commandline(["--serial5=sim:vicon:"])
|
||||||
|
|
||||||
"""Setup parameters including switching to EKF3"""
|
# switch to use GPS
|
||||||
self.context_push()
|
self.set_rc(8, 1000)
|
||||||
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
|
# ensure we can get a global position:
|
||||||
self.set_rc(8, 1000)
|
self.poll_home_position(timeout=120)
|
||||||
|
|
||||||
# ensure we can get a global position:
|
# record starting position
|
||||||
self.poll_home_position(timeout=120)
|
old_pos = self.get_global_position_int()
|
||||||
|
print("old_pos=%s" % str(old_pos))
|
||||||
|
|
||||||
# record starting position
|
# align vicon yaw with ahrs heading
|
||||||
old_pos = self.get_global_position_int()
|
self.set_rc(7, 2000)
|
||||||
print("old_pos=%s" % str(old_pos))
|
|
||||||
|
|
||||||
# align vicon yaw with ahrs heading
|
# takeoff to 10m in Loiter
|
||||||
self.set_rc(7, 2000)
|
self.progress("Moving to ensure location is tracked")
|
||||||
|
self.takeoff(10, mode="LOITER", require_absolute=True, timeout=720)
|
||||||
|
|
||||||
# takeoff to 10m in Loiter
|
# fly forward in Loiter
|
||||||
self.progress("Moving to ensure location is tracked")
|
self.set_rc(2, 1300)
|
||||||
self.takeoff(10, mode="LOITER", require_absolute=True, timeout=720)
|
|
||||||
|
|
||||||
# fly forward in Loiter
|
# disable vicon
|
||||||
self.set_rc(2, 1300)
|
self.set_parameter("SIM_VICON_FAIL", 1)
|
||||||
|
|
||||||
# disable vicon
|
# ensure vehicle remain in Loiter for 15 seconds
|
||||||
self.set_parameter("SIM_VICON_FAIL", 1)
|
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
|
# re-enable vicon
|
||||||
tstart = self.get_sim_time()
|
self.set_parameter("SIM_VICON_FAIL", 0)
|
||||||
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
|
# switch to vicon, disable GPS and wait 10sec to ensure vehicle remains in Loiter
|
||||||
self.set_parameter("SIM_VICON_FAIL", 0)
|
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
|
# ensure vehicle remain in Loiter for 15 seconds
|
||||||
self.set_rc(8, 1500)
|
tstart = self.get_sim_time()
|
||||||
self.set_parameter("GPS1_TYPE", 0)
|
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
|
# RTL and check vehicle arrives within 10m of home
|
||||||
tstart = self.get_sim_time()
|
self.set_rc(2, 1500)
|
||||||
while self.get_sim_time() - tstart < 15:
|
self.do_RTL()
|
||||||
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
|
|
||||||
|
|
||||||
def RTLSpeed(self):
|
def RTLSpeed(self):
|
||||||
"""Test RTL Speed parameters"""
|
"""Test RTL Speed parameters"""
|
||||||
@ -3885,9 +3856,6 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
|
|||||||
|
|
||||||
def RangeFinder(self):
|
def RangeFinder(self):
|
||||||
'''Test RangeFinder Basic Functionality'''
|
'''Test RangeFinder Basic Functionality'''
|
||||||
ex = None
|
|
||||||
self.context_push()
|
|
||||||
|
|
||||||
self.progress("Making sure we don't ordinarily get RANGEFINDER")
|
self.progress("Making sure we don't ordinarily get RANGEFINDER")
|
||||||
m = self.mav.recv_match(type='RANGEFINDER',
|
m = self.mav.recv_match(type='RANGEFINDER',
|
||||||
blocking=True,
|
blocking=True,
|
||||||
@ -3903,68 +3871,59 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
|
|||||||
if self.current_onboard_log_contains_message("RFND"):
|
if self.current_onboard_log_contains_message("RFND"):
|
||||||
raise NotAchievedException("Found unexpected RFND message")
|
raise NotAchievedException("Found unexpected RFND message")
|
||||||
|
|
||||||
try:
|
self.set_analog_rangefinder_parameters()
|
||||||
self.set_analog_rangefinder_parameters()
|
self.set_parameter("RC9_OPTION", 10) # rangefinder
|
||||||
self.set_parameter("RC9_OPTION", 10) # rangefinder
|
self.set_rc(9, 2000)
|
||||||
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()
|
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):
|
def SplineTerrain(self):
|
||||||
'''Test Splines and Terrain'''
|
'''Test Splines and Terrain'''
|
||||||
|
|||||||
@ -86,86 +86,76 @@ class AutoTestRover(vehicle_test_suite.TestSuite):
|
|||||||
def DriveSquare(self, side=50):
|
def DriveSquare(self, side=50):
|
||||||
"""Learn/Drive Square with Ch7 option"""
|
"""Learn/Drive Square with Ch7 option"""
|
||||||
|
|
||||||
self.context_push()
|
self.progress("TEST SQUARE")
|
||||||
ex = None
|
self.set_parameters({
|
||||||
try:
|
"RC7_OPTION": 7,
|
||||||
self.progress("TEST SQUARE")
|
"RC9_OPTION": 58,
|
||||||
self.set_parameters({
|
})
|
||||||
"RC7_OPTION": 7,
|
|
||||||
"RC9_OPTION": 58,
|
|
||||||
})
|
|
||||||
|
|
||||||
self.change_mode('MANUAL')
|
self.change_mode('MANUAL')
|
||||||
|
|
||||||
self.wait_ready_to_arm()
|
self.wait_ready_to_arm()
|
||||||
self.arm_vehicle()
|
self.arm_vehicle()
|
||||||
|
|
||||||
self.clear_wp(9)
|
self.clear_wp(9)
|
||||||
|
|
||||||
# first aim north
|
# first aim north
|
||||||
self.progress("\nTurn right towards north")
|
self.progress("\nTurn right towards north")
|
||||||
self.reach_heading_manual(10)
|
self.reach_heading_manual(10)
|
||||||
# save bottom left corner of box as home AND waypoint
|
# save bottom left corner of box as home AND waypoint
|
||||||
self.progress("Save HOME")
|
self.progress("Save HOME")
|
||||||
self.save_wp()
|
self.save_wp()
|
||||||
|
|
||||||
self.progress("Save WP")
|
self.progress("Save WP")
|
||||||
self.save_wp()
|
self.save_wp()
|
||||||
|
|
||||||
# pitch forward to fly north
|
# pitch forward to fly north
|
||||||
self.progress("\nGoing north %u meters" % side)
|
self.progress("\nGoing north %u meters" % side)
|
||||||
self.reach_distance_manual(side)
|
self.reach_distance_manual(side)
|
||||||
# save top left corner of square as waypoint
|
# save top left corner of square as waypoint
|
||||||
self.progress("Save WP")
|
self.progress("Save WP")
|
||||||
self.save_wp()
|
self.save_wp()
|
||||||
|
|
||||||
# roll right to fly east
|
# roll right to fly east
|
||||||
self.progress("\nGoing east %u meters" % side)
|
self.progress("\nGoing east %u meters" % side)
|
||||||
self.reach_heading_manual(100)
|
self.reach_heading_manual(100)
|
||||||
self.reach_distance_manual(side)
|
self.reach_distance_manual(side)
|
||||||
# save top right corner of square as waypoint
|
# save top right corner of square as waypoint
|
||||||
self.progress("Save WP")
|
self.progress("Save WP")
|
||||||
self.save_wp()
|
self.save_wp()
|
||||||
|
|
||||||
# pitch back to fly south
|
# pitch back to fly south
|
||||||
self.progress("\nGoing south %u meters" % side)
|
self.progress("\nGoing south %u meters" % side)
|
||||||
self.reach_heading_manual(190)
|
self.reach_heading_manual(190)
|
||||||
self.reach_distance_manual(side)
|
self.reach_distance_manual(side)
|
||||||
# save bottom right corner of square as waypoint
|
# save bottom right corner of square as waypoint
|
||||||
self.progress("Save WP")
|
self.progress("Save WP")
|
||||||
self.save_wp()
|
self.save_wp()
|
||||||
|
|
||||||
# roll left to fly west
|
# roll left to fly west
|
||||||
self.progress("\nGoing west %u meters" % side)
|
self.progress("\nGoing west %u meters" % side)
|
||||||
self.reach_heading_manual(280)
|
self.reach_heading_manual(280)
|
||||||
self.reach_distance_manual(side)
|
self.reach_distance_manual(side)
|
||||||
# save bottom left corner of square (should be near home) as waypoint
|
# save bottom left corner of square (should be near home) as waypoint
|
||||||
self.progress("Save WP")
|
self.progress("Save WP")
|
||||||
self.save_wp()
|
self.save_wp()
|
||||||
|
|
||||||
self.progress("Checking number of saved waypoints")
|
self.progress("Checking number of saved waypoints")
|
||||||
mavproxy = self.start_mavproxy()
|
mavproxy = self.start_mavproxy()
|
||||||
num_wp = self.save_mission_to_file_using_mavproxy(
|
num_wp = self.save_mission_to_file_using_mavproxy(
|
||||||
mavproxy,
|
mavproxy,
|
||||||
os.path.join(testdir, "ch7_mission.txt"))
|
os.path.join(testdir, "ch7_mission.txt"))
|
||||||
self.stop_mavproxy(mavproxy)
|
self.stop_mavproxy(mavproxy)
|
||||||
expected = 7 # home + 6 toggled in
|
expected = 7 # home + 6 toggled in
|
||||||
if num_wp != expected:
|
if num_wp != expected:
|
||||||
raise NotAchievedException("Did not get %u waypoints; got %u" %
|
raise NotAchievedException("Did not get %u waypoints; got %u" %
|
||||||
(expected, num_wp))
|
(expected, num_wp))
|
||||||
|
|
||||||
# TODO: actually drive the mission
|
# TODO: actually drive the mission
|
||||||
|
|
||||||
self.clear_wp(9)
|
self.clear_wp(9)
|
||||||
except Exception as e:
|
|
||||||
self.print_exception_caught(e)
|
|
||||||
ex = e
|
|
||||||
|
|
||||||
self.disarm_vehicle()
|
self.disarm_vehicle()
|
||||||
self.context_pop()
|
|
||||||
|
|
||||||
if ex:
|
|
||||||
raise ex
|
|
||||||
|
|
||||||
def drive_left_circuit(self):
|
def drive_left_circuit(self):
|
||||||
"""Drive a left circuit, 50m on a side."""
|
"""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):
|
def AP_Proximity_MAV(self):
|
||||||
'''Test MAV proximity backend'''
|
'''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.set_parameters({
|
||||||
self.send_obstacle_distances_expect_distance_sensor_messages(
|
"PRX1_TYPE": 2, # AP_Proximity_MAV
|
||||||
{
|
"OA_TYPE": 2, # dijkstra
|
||||||
"distances": [234],
|
"OA_DB_OUTPUT": 3, # send all items
|
||||||
"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.reboot_sitl()
|
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):
|
def SendToComponents(self):
|
||||||
'''Test ArduPilot send_to_components function'''
|
'''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):
|
def AutoDock(self):
|
||||||
'''Test automatic docking of rover for multiple FOVs of simulated beacon'''
|
'''Test automatic docking of rover for multiple FOVs of simulated beacon'''
|
||||||
self.context_push()
|
|
||||||
|
|
||||||
self.set_parameters({
|
self.set_parameters({
|
||||||
"PLND_ENABLED": 1,
|
"PLND_ENABLED": 1,
|
||||||
"PLND_TYPE": 4,
|
"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
|
for type in range(0, 3): # CYLINDRICAL FOV, CONICAL FOV, SPHERICAL FOV
|
||||||
ex = None
|
self.set_parameter("SIM_PLD_TYPE", type)
|
||||||
try:
|
self.reboot_sitl()
|
||||||
self.set_parameter("SIM_PLD_TYPE", type)
|
self.change_mode('GUIDED')
|
||||||
self.reboot_sitl()
|
self.wait_ready_to_arm()
|
||||||
self.change_mode('GUIDED')
|
self.arm_vehicle()
|
||||||
self.wait_ready_to_arm()
|
initial_position = self.offset_location_ne(target, -20, -2)
|
||||||
self.arm_vehicle()
|
self.drive_to_location(initial_position)
|
||||||
initial_position = self.offset_location_ne(target, -20, -2)
|
self.change_mode(8) # DOCK mode
|
||||||
self.drive_to_location(initial_position)
|
max_delta = 1
|
||||||
self.change_mode(8) # DOCK mode
|
self.wait_distance_to_location(target, 0, max_delta, timeout=180)
|
||||||
max_delta = 1
|
self.disarm_vehicle()
|
||||||
self.wait_distance_to_location(target, 0, max_delta, timeout=180)
|
self.assert_receive_message('GLOBAL_POSITION_INT')
|
||||||
self.disarm_vehicle()
|
new_pos = self.mav.location()
|
||||||
self.assert_receive_message('GLOBAL_POSITION_INT')
|
delta = abs(self.get_distance(target, new_pos) - stopping_dist)
|
||||||
new_pos = self.mav.location()
|
self.progress("Docked %f metres from stopping point" % delta)
|
||||||
delta = abs(self.get_distance(target, new_pos) - stopping_dist)
|
if delta > max_delta:
|
||||||
self.progress("Docked %f metres from stopping point" % delta)
|
raise NotAchievedException("Did not dock close enough to stopping point (%fm > %fm" % (delta, max_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"):
|
if not self.current_onboard_log_contains_message("PL"):
|
||||||
raise NotAchievedException("Did not see expected PL message")
|
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")
|
self.progress("All done")
|
||||||
|
|
||||||
def PrivateChannel(self):
|
def PrivateChannel(self):
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user