aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

autotest: add test for proximity sensors

Browse Source
This commit is contained in:
Peter Barker 2021-12-11 00:20:28 +11:00 committed by Peter Barker
parent 23ea84bf32
commit 17915faa98
1 changed files with 81 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
Tools/autotest/arducopter.py
View File

@ -5768,6 +5768,83 @@ class AutoTestCopter(AutoTest):
if ex is not None:
raise ex
def ProximitySensors(self):
'''ensure proximity sensors return appropriate data'''
self.set_parameters({
"SERIAL5_PROTOCOL": 11,
"OA_DB_OUTPUT": 3,
"OA_TYPE": 2,
})
sensors = [ # tuples of name, prx_type
('sf45b', 8, {
mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 292,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 257,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 1130,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 1283,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 627,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 967,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 760,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 762,
}),
('rplidara2', 5, {
mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 277,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 256,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 1130,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 1288,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 626,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 970,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 762,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 790,
}),
('terarangertower', 3, {
mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 450,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 282,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 450,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 450,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 450,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 450,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 450,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 450,
}),
]
# the following is a "magic" location SITL understands which
# has some posts near it:
home_string = "%s,%s,%s,%s" % (51.8752066, 14.6487840, 54.15, 0)
for (name, prx_type, expected_distances) in sensors:
self.start_subtest("Testing %s" % name)
self.set_parameter("PRX_TYPE", prx_type)
self.customise_SITL_commandline([
"--uartF=sim:%s:" % name,
"--home", home_string,
])
self.wait_ready_to_arm()
expected_distances_copy = copy.copy(expected_distances)
tstart = self.get_sim_time()
failed = False
wants = []
gots = []
while True:
if self.get_sim_time_cached() - tstart > 30:
raise AutoTestTimeoutException("Failed to get distances")
if len(expected_distances_copy.keys()) == 0:
break
m = self.assert_receive_message("DISTANCE_SENSOR")
if m.orientation not in expected_distances_copy:
continue
got = m.current_distance
want = expected_distances_copy[m.orientation]
wants.append(want)
gots.append(got)
if abs(want - got) > 5:
failed = True
del expected_distances_copy[m.orientation]
if failed:
raise NotAchievedException(
"Distance too great (%s) (want=%s != got=%s)" %
(name, wants, gots))
def fly_proximity_avoidance_test_alt_no_avoid(self):
self.start_subtest("Alt-no-avoid")
self.context_push()
@ -8453,6 +8530,10 @@ class AutoTestCopter(AutoTest):
"Test FETtecESC",
self.FETtecESC),
Test("ProximitySensors",
"Test Proximity Sensors",
self.ProximitySensors),
Test("GroundEffectCompensation_touchDownExpected",
"Test EKF's handling of touchdown-expected",
self.GroundEffectCompensation_touchDownExpected),