diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index ad30751adc..6d37a3d101 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -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),