Tools: allow for more than 327m range rangefinders

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

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

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

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);
 }

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

Tools/autotest/arducopter.py

@@ -2865,7 +2865,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()
 
@@ -3039,7 +3039,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,
@@ -4024,7 +4024,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):
@@ -8390,7 +8392,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)
@@ -8655,10 +8657,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()
@@ -8800,6 +8802,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
@@ -10753,6 +10839,7 @@ def tests1e(self):
              self.ModeFollow,
              self.RangeFinderDrivers,
              self.RangeFinderDriversMaxAlt,
+             self.RangeFinderDriversLongRange,
              self.MaxBotixI2CXL,
              self.MAVProximity,
              self.ParameterValidation,

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)

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
+SIM_FLOW_ENABLE 1

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
+RNGFND1_SCALING 12.12