diff --git a/Tools/Frame_params/Parrot_Disco/Parrot_Disco.param b/Tools/Frame_params/Parrot_Disco/Parrot_Disco.param index 0d26d2c7c84cf..9f88fe5e11f08 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 44b9baeea3f99..c9e84cf411aa7 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 85fce24f5652e..e8453feb39e27 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/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index cbdca233afaf6..fb49e339d1bd7 100644 --- a/Tools/Replay/LR_MsgHandler.cpp +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -205,14 +205,98 @@ void LR_MsgHandler_RBRI::process_message(uint8_t *msgbytes) AP::dal().handle_message(msg); } +// RRNH - Replay RaNgefinder Header constructor +LR_MsgHandler_RRNH::LR_MsgHandler_RRNH(struct log_Format &_f) : + LR_MsgHandler(_f) +{ + // the checksum here is the checksum of the old "struct log_Format + // RRNH" before the type was changed from int16_t to float. It + // corresponds to log_RRNH_with_int16_max_distance, below. + if (strncmp(_f.format, "hhB", ARRAY_SIZE(_f.format)) == 0 && + strncmp(_f.labels, "GCl,MaxD,NumSensors", ARRAY_SIZE(_f.labels)) == 0) { + distances_int16_to_float_transform_required = true; + } +} + +void LR_MsgHandler_RRNH::process_message_int16_to_float_transformed(uint8_t *msgbytes) +{ + // this structure was copied out of AP_DAL.cpp before being + // modified there. _end has been trimmed out + + struct log_RRNH_with_int16_t_distance { + // this is rotation-pitch-270! + int16_t ground_clearance_cm; + int16_t max_distance_cm; + uint8_t num_sensors; + }; + + // note that if fields are added to RRNH message then this + // will be incorrect! + const log_RRNH_with_int16_t_distance &tmp = *((log_RRNH_with_int16_t_distance*)&msgbytes[3]); + const log_RRNH msg { + ground_clearance : tmp.ground_clearance_cm * 0.01, + max_distance : tmp.max_distance_cm * 0.01, + num_sensors : tmp.num_sensors, + }; + AP::dal().handle_message(msg); +} + void LR_MsgHandler_RRNH::process_message(uint8_t *msgbytes) { + if (distances_int16_to_float_transform_required) { + process_message_int16_to_float_transformed(msgbytes); + return; + } + MSG_CREATE(RRNH, msgbytes); AP::dal().handle_message(msg); } +// RRNI - Replay RaNgefinder Instance constructor +LR_MsgHandler_RRNI::LR_MsgHandler_RRNI(struct log_Format &_f) : + LR_MsgHandler(_f) +{ + // the checksum here is the checksum of the old "struct log_Format + // RRNH" before the type was changed from int16_t to float. It + // corresponds to log_RRNH_with_int16_max_distance, below. + if (strncmp(_f.format, "fffHBBB", ARRAY_SIZE(_f.format)) == 0 && + strncmp(_f.labels, "PX,PY,PZ,Dist,Orient,Status,I", ARRAY_SIZE(_f.labels)) == 0) { + distances_int16_to_float_transform_required = true; + } +} + +void LR_MsgHandler_RRNI::process_message_int16_to_float_transformed(uint8_t *msgbytes) +{ + // this structure was copied out of AP_DAL.cpp before being + // modified there. _end has been trimmed out + struct log_RRNI_with_int16_t_distance { + Vector3f pos_offset; + uint16_t distance; + uint8_t orientation; + uint8_t status; + uint8_t instance; + }; + + // note that if fields are added to RRNI message then this + // will be incorrect! + const log_RRNI_with_int16_t_distance &tmp = *((log_RRNI_with_int16_t_distance*)&msgbytes[3]); + const log_RRNI msg { + pos_offset : tmp.pos_offset, + distance : tmp.distance * 0.01, + orientation : tmp.orientation, + status : tmp.status, + instance : tmp.instance + }; + AP::dal().handle_message(msg); +} + void LR_MsgHandler_RRNI::process_message(uint8_t *msgbytes) { + if (distances_int16_to_float_transform_required) { + process_message_int16_to_float_transformed(msgbytes); + return; + } + MSG_CREATE(RRNI, msgbytes); AP::dal().handle_message(msg); } diff --git a/Tools/Replay/LR_MsgHandler.h b/Tools/Replay/LR_MsgHandler.h index 4e30a6571d35c..b256545282772 100644 --- a/Tools/Replay/LR_MsgHandler.h +++ b/Tools/Replay/LR_MsgHandler.h @@ -176,17 +176,28 @@ class LR_MsgHandler_RBRI : public LR_MsgHandler void process_message(uint8_t *msg) override; }; +// RRNH - Replay RaNgefinder Header class LR_MsgHandler_RRNH : public LR_MsgHandler { public: - using LR_MsgHandler::LR_MsgHandler; + LR_MsgHandler_RRNH(struct log_Format &_f); void process_message(uint8_t *msg) override; +private: + // code to cope with transformation from older log format (16-bit + // distances) to new format (32-bit distances): + bool distances_int16_to_float_transform_required; + void process_message_int16_to_float_transformed(uint8_t *msgbytes); }; class LR_MsgHandler_RRNI : public LR_MsgHandler { public: - using LR_MsgHandler::LR_MsgHandler; + LR_MsgHandler_RRNI(struct log_Format &_f); void process_message(uint8_t *msg) override; +private: + // code to cope with transformation from older log format (16-bit + // distances) to new format (32-bit distances): + bool distances_int16_to_float_transform_required; + void process_message_int16_to_float_transformed(uint8_t *msgbytes); }; class LR_MsgHandler_RGPH : public LR_MsgHandler diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 2f2a9ca811c55..0abcd1c0de642 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -2870,7 +2870,7 @@ def EK3_RNG_USE_HGT(self): 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 @@ def CANGPSCopterMission(self): "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 @@ def test_rangefinder_switchover(self): 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 @@ def fly_guided_move_local(self, x, y, z_up, timeout=100): 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): @@ -8512,7 +8514,7 @@ def fly_rangefinder_mavlink_distance_sensor(self): 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) @@ -8777,10 +8779,10 @@ def MaxBotixI2CXL(self): 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() @@ -8922,6 +8924,90 @@ def RangeFinderDriversMaxAlt(self): self.do_RTL() + def RangeFinderDriversLongRange(self): + '''test rangefinder above 327m''' + 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 ShipTakeoff(self): '''Fly Simulated Ship Takeoff''' # test ship takeoff @@ -10866,6 +10952,7 @@ def tests1e(self): self.ModeFollow, self.RangeFinderDrivers, self.RangeFinderDriversMaxAlt, + self.RangeFinderDriversLongRange, self.MaxBotixI2CXL, self.MAVProximity, self.ParameterValidation, diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 5947640790b44..88b498721f6c8 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -16,7 +16,6 @@ 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 @@ def RngfndQuality(self): "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 @@ def watch_distance_maintained(self, delta=0.3, timeout=5.0): 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 @@ def prepare_synthetic_seafloor_test(self, sea_floor_depth, rf_target): "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 @@ def MAV_CMD_DO_REPOSITION(self): 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 162c037074101..90f4ee41937c6 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 0005a89571eea..1963fe33b4987 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 9e586a53fdd18..7fba567f3fdb0 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 d1674bb220f22..ca13f126fdbac 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 2f6df0c33ba05..90711c5bed888 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 8bb2195a7f783..2596901289a66 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 65615aa5f2abd..bf0b9ddfafd00 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 9415f055bdedc..c5305222954ab 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 df57aea2e9064..b3789db7da7b0 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 95e72e276f3e1..2a7e26c8544ec 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -1523,7 +1523,7 @@ def PrecisionLanding(self): "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 6ff5fc7c342d5..4b967b548d630 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -5825,8 +5825,8 @@ def send_mavlink_run_prearms_command(self): 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, } @@ -7211,6 +7211,8 @@ def assert_rangefinder_distance_between(self, dist_min, dist_max): 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') @@ -7218,6 +7220,12 @@ def assert_distance_sensor_quality(self, quality): raise NotAchievedException("Unexpected quality; want=%f got=%f" % (quality, m.signal_quality)) + def assert_distance_sensor_distance(self, distance, epsilon=0.5): + m = self.assert_receive_message('DISTANCE_SENSOR') + + if abs(m.distance_cm * 0.01 - distance) > epsilon: + raise NotAchievedException("Unexpected distance; want={distance} got={m.distance_cm*0.01}") + def get_rangefinder_distance(self): m = self.assert_receive_message('RANGEFINDER', timeout=5) return m.distance