Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

autotest: add test for MAVProxy proximity sensor

Browse Source
This commit is contained in:
Peter Barker 2021-02-04 10:59:02 +11:00 committed by Peter Barker
parent acfe815305
commit 393ec5d6f7
1 changed files with 104 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
Tools/autotest/arducopter.py
View File

@ -5438,6 +5438,105 @@ class AutoTestCopter(AutoTest):
if not self.current_onboard_log_contains_message("RFND"):
raise NotAchievedException("No RFND messages in log")
def fly_proximity_mavlink_distance_sensor(self):
self.start_subtest("Test mavlink proximity sensor using DISTANCE_SENSOR messages") # noqa
self.context_push()
ex = None
try:
self.set_parameter("SERIAL5_PROTOCOL", 1)
self.set_parameter("PRX_TYPE", 2) # mavlink
self.reboot_sitl()
self.progress("Should be unhealthy while we don't send messages")
self.assert_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_PROXIMITY, True, True, False)
self.progress("Should be healthy while we're sending good messages")
tstart = self.get_sim_time()
while True:
if self.get_sim_time() - tstart > 5:
raise NotAchievedException("Sensor did not come good")
self.mav.mav.distance_sensor_send(
0, # time_boot_ms
10, # min_distance cm
50, # max_distance cm
20, # current_distance cm
mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER, # type
21, # id
mavutil.mavlink.MAV_SENSOR_ROTATION_NONE, # orientation
255 # covariance
)
if self.sensor_has_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_PROXIMITY, True, True, True):
self.progress("Sensor has good state")
break
self.delay_sim_time(0.1)
self.progress("Should be unhealthy again if we stop sending messages")
self.delay_sim_time(1)
self.assert_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_PROXIMITY, True, True, False)
# now make sure we get echoed back the same sorts of things we send:
# distances are in cm
distance_map = {
mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 30,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 35,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 20,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 15,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 70,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 80,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 10,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 90,
}
wanted_distances = copy.copy(distance_map)
sensor_enum = mavutil.mavlink.enums["MAV_SENSOR_ORIENTATION"]
def my_message_hook(mav, m):
if m.get_type() != 'DISTANCE_SENSOR':
return
self.progress("Got (%s)" % str(m))
want = distance_map[m.orientation]
got = m.current_distance
# ArduPilot's floating point conversions make it imprecise:
delta = abs(want-got)
if delta > 1:
self.progress(
"Wrong distance (%s): want=%f got=%f" %
(sensor_enum[m.orientation].name, want, got))
return
if m.orientation not in wanted_distances:
return
self.progress(
"Correct distance (%s): want=%f got=%f" %
(sensor_enum[m.orientation].name, want, got))
del wanted_distances[m.orientation]
self.install_message_hook_context(my_message_hook)
tstart = self.get_sim_time()
while True:
if self.get_sim_time() - tstart > 5:
raise NotAchievedException("Sensor did not give right distances") # noqa
for (orient, dist) in distance_map.items():
self.mav.mav.distance_sensor_send(
0, # time_boot_ms
10, # min_distance cm
90, # max_distance cm
dist, # current_distance cm
mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER, # type
21, # id
orient, # orientation
255 # covariance
)
self.wait_heartbeat()
if len(wanted_distances.keys()) == 0:
break
except Exception as e:
self.progress("Caught exception: %s" %
self.get_exception_stacktrace(e))
ex = e
self.context_pop()
self.reboot_sitl()
if ex is not None:
raise ex
def fly_rangefinder_mavlink_distance_sensor(self):
self.start_subtest("Test mavlink rangefinder using DISTANCE_SENSOR messages")
self.context_push()
@ -6229,6 +6328,11 @@ class AutoTestCopter(AutoTest):
"Test maxbotix rangefinder drivers",
self.fly_rangefinder_driver_maxbotix), #62s
("MAVProximity",
"Test MAVLink proximity driver",
self.fly_proximity_mavlink_distance_sensor,
),
("ParameterValidation",
"Test parameters are checked for validity",
self.test_parameter_validation),