diff --git a/Tools/Frame_params/Parrot_Disco/Parrot_Disco.param b/Tools/Frame_params/Parrot_Disco/Parrot_Disco.param index 0d26d2c7c8..9f88fe5e11 100644 --- a/Tools/Frame_params/Parrot_Disco/Parrot_Disco.param +++ b/Tools/Frame_params/Parrot_Disco/Parrot_Disco.param @@ -43,8 +43,8 @@ RLL_RATE_P 0.040758 RLL2SRV_RMAX 75.000000 RLL2SRV_TCONST 0.450000 RNGFND_LANDING 1 -RNGFND_MAX_CM 700 -RNGFND_MIN_CM 20 +RNGFND_MAX 7.00 +RNGFND_MIN 0.20 RNGFND_PIN -1 RNGFND_SCALING 1 RNGFND_TYPE 9 diff --git a/Tools/Frame_params/deset-mapping-boat.param b/Tools/Frame_params/deset-mapping-boat.param index 44b9baeea3..c9e84cf411 100644 --- a/Tools/Frame_params/deset-mapping-boat.param +++ b/Tools/Frame_params/deset-mapping-boat.param @@ -25,8 +25,8 @@ MOT_SLEWRATE,25 MOT_VEC_ANGLEMAX,30 NAVL1_DAMPING,0.8 NAVL1_PERIOD,16 -RNGFND1_MAX_CM,20000 -RNGFND1_MIN_CM,0 +RNGFND1_MAX,200.00 +RNGFND1_MIN,0 RNGFND1_TYPE,17 SERIAL2_BAUD,19 SERIAL2_PROTOCOL,39 diff --git a/Tools/Frame_params/eLAB_EX700_AC34.param b/Tools/Frame_params/eLAB_EX700_AC34.param index 85fce24f56..e8453feb39 100644 --- a/Tools/Frame_params/eLAB_EX700_AC34.param +++ b/Tools/Frame_params/eLAB_EX700_AC34.param @@ -39,8 +39,8 @@ MOT_THST_HOVER,0.25 MOT_THST_EXPO,0.7 MOT_BAT_VOLT_MAX,25.2 MOT_BAT_VOLT_MIN,21.0 -RNGFND_MAX_CM,5000 -RNGFND_MIN_CM,5 +RNGFND_MAX,50.00 +RNGFND_MIN,0.05 RNGFND_SCALING,1 RNGFND_TYPE,8 PILOT_THR_BHV,1 diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index d11aaa1e63..07f920e6cd 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -2870,7 +2870,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.set_analog_rangefinder_parameters() # set use-height to 20m (the parameter is a percentage of max range) self.set_parameters({ - 'EK3_RNG_USE_HGT': 200000 / self.get_parameter('RNGFND1_MAX_CM'), + 'EK3_RNG_USE_HGT': (20 / self.get_parameter('RNGFND1_MAX')) * 100, }) self.reboot_sitl() @@ -3044,7 +3044,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): "COMPASS_USE3" : 0, # use DroneCAN rangefinder "RNGFND1_TYPE" : 24, - "RNGFND1_MAX_CM" : 11000, + "RNGFND1_MAX" : 110.00, # use DroneCAN battery monitoring, and enforce with a arming voltage "BATT_MONITOR" : 8, "BATT_ARM_VOLT" : 12.0, @@ -4026,7 +4026,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.set_analog_rangefinder_parameters() self.set_parameters({ - "RNGFND1_MAX_CM": 1500 + "RNGFND1_MAX": 15.00 }) # configure EKF to use rangefinder for altitude at low altitudes @@ -4757,7 +4757,9 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): while True: if self.get_sim_time_cached() - tstart > timeout: raise NotAchievedException("Did not reach destination") - if self.distance_to_local_position((x, y, -z_up)) < 1: + dist = self.distance_to_local_position((x, y, -z_up)) + if dist < 1: + self.progress(f"Reach distance ({dist})") break def test_guided_local_position_target(self, x, y, z_up): @@ -8522,7 +8524,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.set_parameter("SERIAL5_PROTOCOL", 1) self.set_parameter("RNGFND1_TYPE", 10) self.reboot_sitl() - self.set_parameter("RNGFND1_MAX_CM", 32767) + self.set_parameter("RNGFND1_MAX", 327.67) self.progress("Should be unhealthy while we don't send messages") self.assert_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION, True, True, False) @@ -8787,10 +8789,10 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.set_parameters({ "RNGFND1_TYPE": 2, # maxbotix "RNGFND1_ADDR": 0x70, - "RNGFND1_MIN_CM": 150, + "RNGFND1_MIN": 1.50, "RNGFND2_TYPE": 2, # maxbotix "RNGFND2_ADDR": 0x71, - "RNGFND2_MIN_CM": 250, + "RNGFND2_MIN": 2.50, }) self.reboot_sitl() self.do_timesync_roundtrip() @@ -8932,6 +8934,176 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.do_RTL() + def RangeFinderDriversLongRange(self): + '''test rangefinder above 327m''' + # FIXME: when we get a driver+simulator for a rangefinder with + # >327m range change this to use that driver + self.set_parameters({ + "SERIAL4_PROTOCOL": 9, + "RNGFND1_TYPE": 19, # BenewakeTF02 + "WPNAV_SPEED_UP": 1000, # cm/s + }) + self.customise_SITL_commandline([ + "--serial4=sim:benewake_tf02", + ]) + + text_good = "GOOD" + text_out_of_range_high = "OUT_OF_RANGE_HIGH" + + rf_bit = mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION + + alt = 3 + self.takeoff(alt, mode='GUIDED') + self.assert_parameter_value("RNGFND1_MAX", 7.00) + self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True) + m = self.assert_receive_message('DISTANCE_SENSOR', verbose=True) + if abs(m.current_distance * 0.01 - alt) > 1: + raise NotAchievedException(f"Expected {alt}m range") + self.assert_parameter_value("RNGFND1_MAX", m.max_distance * 0.01, epsilon=0.00001) + self.assert_parameter_value("RNGFND1_MIN", m.min_distance * 0.01, epsilon=0.00001) + + self.send_statustext(text_good) + + alt = 10 + self.fly_guided_move_local(0, 0, alt) + self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True) + m = self.assert_receive_message('DISTANCE_SENSOR', verbose=True) + if abs(m.current_distance * 0.01 - alt) > 1: + raise NotAchievedException(f"Expected {alt}m range") + self.assert_parameter_value("RNGFND1_MAX", m.max_distance * 0.01, epsilon=0.00001) + self.assert_parameter_value("RNGFND1_MIN", m.min_distance * 0.01, epsilon=0.00001) + self.send_statustext(text_out_of_range_high) + + self.progress("Moving the goalposts") + self.set_parameter("RNGFND1_MAX", 20) + self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True) + m = self.assert_receive_message('DISTANCE_SENSOR', verbose=True) + if abs(m.current_distance * 0.01 - alt) > 1: + raise NotAchievedException(f"Expected {alt}m range") + self.assert_parameter_value("RNGFND1_MAX", m.max_distance * 0.01, epsilon=0.00001) + self.assert_parameter_value("RNGFND1_MIN", m.min_distance * 0.01, epsilon=0.00001) + self.send_statustext(text_good) + self.delay_sim_time(2) + + dfreader = self.dfreader_for_current_onboard_log() + + self.do_RTL() + + self.progress("Checking in/out of range markers in log") + required_range = None + count = 0 + while True: + m = dfreader.recv_match(type=["MSG", "RFND"]) + if m is None: + break + m_type = m.get_type() + if m_type == "MSG": + for t in [text_good, text_out_of_range_high]: + if t in m.Message: + required_range = t + continue + if m_type == "RFND": + if required_range is None: + continue + if required_range == text_good: + required_state = 4 + elif required_range == text_out_of_range_high: + required_state = 3 + else: + raise ValueError(f"Unexpected text {required_range}") + if m.Stat != required_state: + raise NotAchievedException(f"Not in expected state want={required_state} got={m.Stat} dist={m.Dist}") + self.progress(f"In expected state {required_range}") + required_range = None + count += 1 + + if count < 3: + raise NotAchievedException("Did not see range markers") + + def RangeFinderSITLLongRange(self): + '''test rangefinder above 327m''' + # FIXME: when we get a driver+simulator for a rangefinder with + # >327m range change this to use that driver + self.set_parameters({ + "RNGFND1_TYPE": 100, # SITL + "WPNAV_SPEED_UP": 1000, # cm/s + "RNGFND1_MAX": 7000, # metres + }) + self.reboot_sitl() + + text_good = "GOOD" + text_high = "HIGH" + + rf_bit = mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION + + alt = 3 + self.takeoff(alt, mode='GUIDED') + self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True) + m = self.assert_receive_message('DISTANCE_SENSOR', verbose=True) + got = m.current_distance * 0.01 + if abs(got - alt) > 1: + raise NotAchievedException(f"Expected {alt}m range {got=}") + + self.send_statustext(text_good) + + alt = 635 + self.fly_guided_move_local(0, 0, alt) + self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True) + m = self.assert_receive_message('DISTANCE_SENSOR', verbose=True) + if abs(m.current_distance * 0.01 - alt) > 1: + raise NotAchievedException(f"Expected {alt}m range") + + self.send_statustext(text_high) + + dfreader = self.dfreader_for_current_onboard_log() + + # fast descent! + def hook(msg, m): + if m.get_type() != 'HEARTBEAT': + return + # tell vehicle to only pay attention to the attitude: + bitmask = 0 + bitmask |= mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE + bitmask |= mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE + bitmask |= mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE + bitmask |= mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE + + self.mav.mav.set_attitude_target_send( + 0, # time_boot_ms + 1, # target sysid + 1, # target compid + 0, # bitmask of things to ignore + mavextra.euler_to_quat([ + math.radians(-180), + math.radians(0), + math.radians(0)]), # att + 0, # roll rate (rad/s) + 0, # pitch rate (rad/s) + 0, # yaw rate (rad/s) + 1 # thrust, 0 to 1, translated to a climb/descent rate + ) + + self.install_message_hook_context(hook) + + self.wait_altitude(0, 30, timeout=200, relative=True) + self.do_RTL() + + self.progress("Checking in/out of range markers in log") + good = False + while True: + m = dfreader.recv_match(type=["MSG", "RFND"]) + if m is None: + break + m_type = m.get_type() + if m_type == "RFND": + if m.Dist < 600: + continue + good = True + break + + if not good: + raise NotAchievedException("Did not see good alt") + def ShipTakeoff(self): '''Fly Simulated Ship Takeoff''' # test ship takeoff @@ -10876,6 +11048,8 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.ModeFollow, self.RangeFinderDrivers, self.RangeFinderDriversMaxAlt, + self.RangeFinderDriversLongRange, + self.RangeFinderSITLLongRange, self.MaxBotixI2CXL, self.MAVProximity, self.ParameterValidation, diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 5947640790..88b498721f 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -16,7 +16,6 @@ from pymavlink import mavutil import vehicle_test_suite from vehicle_test_suite import NotAchievedException from vehicle_test_suite import AutoTestTimeoutException -from vehicle_test_suite import PreconditionFailedException if sys.version_info[0] < 3: ConnectionResetError = AutoTestTimeoutException @@ -196,8 +195,8 @@ class AutoTestSub(vehicle_test_suite.TestSuite): "SCR_ENABLE": 1, "RNGFND1_TYPE": 36, "RNGFND1_ORIENT": 25, - "RNGFND1_MIN_CM": 10, - "RNGFND1_MAX_CM": 5000, + "RNGFND1_MIN": 0.10, + "RNGFND1_MAX": 50.00, }) self.install_example_script_context("rangefinder_quality_test.lua") @@ -244,8 +243,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite): def Surftrak(self): """Test SURFTRAK mode""" - if self.get_parameter('RNGFND1_MAX_CM') != 3000.0: - raise PreconditionFailedException("RNGFND1_MAX_CM is not %g" % 3000.0) + self.assert_parameter_value('RNGFND1_MAX', 30) # Something closer to Bar30 noise self.context_push() @@ -292,8 +290,8 @@ class AutoTestSub(vehicle_test_suite.TestSuite): "SCR_ENABLE": 1, "RNGFND1_TYPE": 36, "RNGFND1_ORIENT": 25, - "RNGFND1_MIN_CM": 10, - "RNGFND1_MAX_CM": 3000, + "RNGFND1_MIN": 0.10, + "RNGFND1_MAX": 30.00, "SCR_USER1": 2, # Configuration bundle "SCR_USER2": sea_floor_depth, # Depth in meters "SCR_USER3": 101, # Output log records @@ -788,8 +786,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite): def TerrainMission(self): """Mission using surface tracking""" - if self.get_parameter('RNGFND1_MAX_CM') != 3000.0: - raise PreconditionFailedException("RNGFND1_MAX_CM is not %g" % 3000.0) + self.assert_parameter_value('RNGFND1_MAX', 30) filename = "terrain_mission.txt" self.progress("Executing mission %s" % filename) diff --git a/Tools/autotest/default_params/copter-optflow.parm b/Tools/autotest/default_params/copter-optflow.parm index 162c037074..90f4ee4193 100644 --- a/Tools/autotest/default_params/copter-optflow.parm +++ b/Tools/autotest/default_params/copter-optflow.parm @@ -5,8 +5,8 @@ EK3_SRC1_VELXY 5 EK3_SRC1_VELZ 0 FLOW_TYPE 10 RNGFND1_TYPE 1 -RNGFND1_MIN_CM 0 -RNGFND1_MAX_CM 4000 +RNGFND1_MIN 0 +RNGFND1_MAX 40.00 RNGFND1_PIN 0 RNGFND1_SCALING 12.12 -SIM_FLOW_ENABLE 1 \ No newline at end of file +SIM_FLOW_ENABLE 1 diff --git a/Tools/autotest/default_params/copter-rangefinder.parm b/Tools/autotest/default_params/copter-rangefinder.parm index 0005a89571..1963fe33b4 100644 --- a/Tools/autotest/default_params/copter-rangefinder.parm +++ b/Tools/autotest/default_params/copter-rangefinder.parm @@ -1,5 +1,5 @@ RNGFND1_TYPE 1 -RNGFND1_MIN_CM 0 -RNGFND1_MAX_CM 4000 +RNGFND1_MIN 0 +RNGFND1_MAX 40.00 RNGFND1_PIN 0 -RNGFND1_SCALING 12.12 \ No newline at end of file +RNGFND1_SCALING 12.12 diff --git a/Tools/autotest/default_params/gazebo-iris.parm b/Tools/autotest/default_params/gazebo-iris.parm index 9e586a53fd..7fba567f3f 100644 --- a/Tools/autotest/default_params/gazebo-iris.parm +++ b/Tools/autotest/default_params/gazebo-iris.parm @@ -10,4 +10,4 @@ SIM_SONAR_SCALE 10 RNGFND1_TYPE 1 RNGFND1_SCALING 10 RNGFND1_PIN 0 -RNGFND1_MAX_CM 5000 +RNGFND1_MAX 50.00 diff --git a/Tools/autotest/default_params/periph.parm b/Tools/autotest/default_params/periph.parm index d1674bb220..ca13f126fd 100644 --- a/Tools/autotest/default_params/periph.parm +++ b/Tools/autotest/default_params/periph.parm @@ -5,7 +5,7 @@ COMPASS_ENABLE 1 BARO_ENABLE 1 ARSPD_TYPE 100 RNGFND1_TYPE 100 -RNGFND1_MAX_CM 12000 +RNGFND1_MAX 120.00 BATT_MONITOR 16 BATT_I2C_BUS 2 diff --git a/Tools/autotest/default_params/quad-can.parm b/Tools/autotest/default_params/quad-can.parm index 2f6df0c33b..90711c5bed 100644 --- a/Tools/autotest/default_params/quad-can.parm +++ b/Tools/autotest/default_params/quad-can.parm @@ -4,5 +4,5 @@ SIM_CAN_SRV_MSK 0xFf SIM_VIB_MOT_MAX 270 GPS1_TYPE 9 RNGFND1_TYPE 24 -RNGFND1_MAX_CM 11000 +RNGFND1_MAX 110.00 BATT_MONITOR 8 diff --git a/Tools/autotest/default_params/quadplane-can.parm b/Tools/autotest/default_params/quadplane-can.parm index 8bb2195a7f..2596901289 100644 --- a/Tools/autotest/default_params/quadplane-can.parm +++ b/Tools/autotest/default_params/quadplane-can.parm @@ -8,6 +8,6 @@ SIM_VIB_MOT_MAX 270 GPS1_TYPE 9 ARSPD_TYPE 8 RNGFND1_TYPE 24 -RNGFND1_MAX_CM 11000 +RNGFND1_MAX 110.00 RNGFND_LANDING 1 BATT_MONITOR 8 diff --git a/Tools/autotest/default_params/sub.parm b/Tools/autotest/default_params/sub.parm index 65615aa5f2..bf0b9ddfaf 100644 --- a/Tools/autotest/default_params/sub.parm +++ b/Tools/autotest/default_params/sub.parm @@ -75,7 +75,7 @@ PSC_POSXY_P 2.5 PSC_VELXY_D 0.8 PSC_VELXY_I 0.5 PSC_VELXY_P 5.0 -RNGFND1_MAX_CM 3000 +RNGFND1_MAX 30.00 RNGFND1_PIN 0 RNGFND1_SCALING 12.12 RNGFND1_TYPE 1 diff --git a/Tools/autotest/default_params/vee-gull_005.param b/Tools/autotest/default_params/vee-gull_005.param index 9415f055bd..c530522295 100644 --- a/Tools/autotest/default_params/vee-gull_005.param +++ b/Tools/autotest/default_params/vee-gull_005.param @@ -465,10 +465,10 @@ RLL2SRV_RMAX,0 RLL2SRV_TCONST,0.5 RNGFND1_ADDR,0 RNGFND1_FUNCTION,0 -RNGFND1_GNDCLEAR,10 +RNGFND1_GNDCLR,0.01 RNGFND1_LANDING,0 -RNGFND1_MAX_CM,700 -RNGFND1_MIN_CM,20 +RNGFND1_MAX,7.00 +RNGFND1_MIN,0.20 RNGFND1_OFFSET,0 RNGFND1_ORIENT,25 RNGFND1_PIN,-1 @@ -483,9 +483,9 @@ RNGFND1_STOP_PIN,-1 RNGFND1_TYPE,0 RNGFND2_ADDR,0 RNGFND2_FUNCTION,0 -RNGFND2_GNDCLEAR,10 -RNGFND2_MAX_CM,700 -RNGFND2_MIN_CM,20 +RNGFND2_GNDCLR,0.01 +RNGFND2_MAX,7.00 +RNGFND2_MIN,0.20 RNGFND2_OFFSET,0 RNGFND2_ORIENT,25 RNGFND2_PIN,-1 diff --git a/Tools/autotest/models/Callisto.param b/Tools/autotest/models/Callisto.param index df57aea2e9..b3789db7da 100644 --- a/Tools/autotest/models/Callisto.param +++ b/Tools/autotest/models/Callisto.param @@ -55,7 +55,7 @@ PSC_ANGLE_MAX,45 PSC_JERK_Z,10 PSC_POSZ_P,0.5 PSC_VELZ_P,2.5 -RNGFND1_MAX_CM,10000 +RNGFND1_MAX,100.00 RNGFND1_PIN,0 RNGFND1_SCALING,12.1212 RNGFND1_TYPE,1 diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 69fdd6eb0c..8e2500a9e0 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -1524,7 +1524,7 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite): "RNGFND1_TYPE": 100, "RNGFND1_PIN" : 0, "RNGFND1_SCALING" : 12.2, - "RNGFND1_MAX_CM" : 5000, + "RNGFND1_MAX" : 50.00, "RNGFND_LANDING" : 1, }) diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index e2022f21b7..9557c0f72c 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -5823,8 +5823,8 @@ class TestSuite(ABC): def analog_rangefinder_parameters(self): return { "RNGFND1_TYPE": 1, - "RNGFND1_MIN_CM": 0, - "RNGFND1_MAX_CM": 4000, + "RNGFND1_MIN": 0, + "RNGFND1_MAX": 40.00, "RNGFND1_SCALING": 12.12, "RNGFND1_PIN": 0, } @@ -7209,6 +7209,8 @@ class TestSuite(ABC): raise NotAchievedException("above max height (%f > %f)" % (m.distance, dist_max)) + self.progress(f"Rangefinder distance {m.distance} is between {dist_min} and {dist_max}") + def assert_distance_sensor_quality(self, quality): m = self.assert_receive_message('DISTANCE_SENSOR')