mirror of https://github.com/ArduPilot/ardupilot
autotest: add test for proximity sensors
This commit is contained in:
parent
23ea84bf32
commit
17915faa98
|
@ -5768,6 +5768,83 @@ class AutoTestCopter(AutoTest):
|
||||||
if ex is not None:
|
if ex is not None:
|
||||||
raise ex
|
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):
|
def fly_proximity_avoidance_test_alt_no_avoid(self):
|
||||||
self.start_subtest("Alt-no-avoid")
|
self.start_subtest("Alt-no-avoid")
|
||||||
self.context_push()
|
self.context_push()
|
||||||
|
@ -8453,6 +8530,10 @@ class AutoTestCopter(AutoTest):
|
||||||
"Test FETtecESC",
|
"Test FETtecESC",
|
||||||
self.FETtecESC),
|
self.FETtecESC),
|
||||||
|
|
||||||
|
Test("ProximitySensors",
|
||||||
|
"Test Proximity Sensors",
|
||||||
|
self.ProximitySensors),
|
||||||
|
|
||||||
Test("GroundEffectCompensation_touchDownExpected",
|
Test("GroundEffectCompensation_touchDownExpected",
|
||||||
"Test EKF's handling of touchdown-expected",
|
"Test EKF's handling of touchdown-expected",
|
||||||
self.GroundEffectCompensation_touchDownExpected),
|
self.GroundEffectCompensation_touchDownExpected),
|
||||||
|
|
Loading…
Reference in New Issue