AP_RangeFinder: allow for more than 327m range

ArduCopter/AP_Arming.cpp

@@ -267,8 +267,8 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
                     return false;
                 }
                 // check if RTL_ALT is higher than rangefinder's max range
-                if (copter.g.rtl_altitude > copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) {
-                    check_failed(ARMING_CHECK_PARAMETERS, display_failure, failure_template, "RTL_ALT>RNGFND_MAX_CM");
+                if (copter.g.rtl_altitude > copter.rangefinder.max_distance_orient(ROTATION_PITCH_270) * 100) {
+                    check_failed(ARMING_CHECK_PARAMETERS, display_failure, failure_template, "RTL_ALT (in cm) above RNGFND_MAX (in metres)");
                     return false;
                 }
 #else

ArduCopter/heli.cpp

@@ -46,7 +46,7 @@ void Copter::check_dynamic_flight(void)
     if (!moving && rangefinder_state.enabled && rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::Good) {
         // when we are more than 2m from the ground with good
         // rangefinder lock consider it to be dynamic flight
-        moving = (rangefinder.distance_cm_orient(ROTATION_PITCH_270) > 200);
+        moving = (rangefinder.distance_orient(ROTATION_PITCH_270) > 2);
     }
 #endif
 

ArduCopter/mode_guided.cpp

@@ -139,7 +139,7 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
 #if AP_RANGEFINDER_ENABLED
     if (wp_nav->rangefinder_used_and_healthy() &&
         wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER &&
-        takeoff_alt_cm < copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) {
+        takeoff_alt_cm < copter.rangefinder.max_distance_orient(ROTATION_PITCH_270)*100) {
         // can't takeoff downwards
         if (takeoff_alt_cm <= copter.rangefinder_state.alt_cm) {
             return false;

ArduPlane/altitude.cpp

@@ -719,10 +719,10 @@ void Plane::rangefinder_height_update(void)
         // to misconfiguration or a faulty sensor
         if (rangefinder_state.in_range_count < 10) {
             if (!is_equal(distance, rangefinder_state.last_distance) &&
-                fabsf(rangefinder_state.initial_range - distance) > 0.05f * rangefinder.max_distance_cm_orient(rangefinder_orientation())*0.01f) {
+                fabsf(rangefinder_state.initial_range - distance) > 0.05f * rangefinder.max_distance_orient(rangefinder_orientation())) {
                 rangefinder_state.in_range_count++;
             }
-            if (fabsf(rangefinder_state.last_distance - distance) > rangefinder.max_distance_cm_orient(rangefinder_orientation())*0.01*0.2) {
+            if (fabsf(rangefinder_state.last_distance - distance) > rangefinder.max_distance_orient(rangefinder_orientation())*0.2) {
                 // changes by more than 20% of full range will reset counter
                 rangefinder_state.in_range_count = 0;
             }

ArduSub/Log.cpp

@@ -15,8 +15,8 @@ struct PACKED log_Control_Tuning {
     float    desired_alt;
     float    inav_alt;
     float    baro_alt;
-    int16_t  desired_rangefinder_alt;
-    int16_t  rangefinder_alt;
+    float    desired_rangefinder_alt;
+    float    rangefinder_alt;
     float    terr_alt;
     int16_t  target_climb_rate;
     int16_t  climb_rate;
@@ -47,8 +47,8 @@ void Sub::Log_Write_Control_Tuning()
         desired_alt         : pos_control.get_pos_target_z_cm() * 0.01f,
         inav_alt            : inertial_nav.get_position_z_up_cm() * 0.01f,
         baro_alt            : barometer.get_altitude(),
-        desired_rangefinder_alt   : (int16_t)mode_surftrak.get_rangefinder_target_cm(),
-        rangefinder_alt           : rangefinder_state.alt_cm,
+        desired_rangefinder_alt   : mode_surftrak.get_rangefinder_target_cm() * 0.01,
+        rangefinder_alt           : rangefinder_state.alt,
         terr_alt            : terr_alt,
         target_climb_rate   : (int16_t)pos_control.get_vel_target_z_cms(),
         climb_rate          : climb_rate
@@ -264,7 +264,7 @@ void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target
 const struct LogStructure Sub::log_structure[] = {
     LOG_COMMON_STRUCTURES,
     { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
-      "CTUN", "Qfffffffccfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00BBBBBB" },
+      "CTUN", "Qffffffffffhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B00BBB" },
     { LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t),         
       "D16",   "QBh",         "TimeUS,Id,Value", "s--", "F--" },
     { LOG_DATA_UINT16_MSG, sizeof(log_Data_UInt16t),         

ArduSub/Sub.h

@@ -145,15 +145,15 @@ class Sub : public AP_Vehicle {
     AP_LeakDetector leak_detector;
 
     struct {
-        bool enabled:1;
-        bool alt_healthy:1; // true if we can trust the altitude from the rangefinder
-        int16_t alt_cm;     // tilt compensated altitude (in cm) from rangefinder
-        int16_t min_cm;     // min rangefinder distance (in cm)
-        int16_t max_cm;     // max rangefinder distance (in cm)
+        bool enabled;
+        bool alt_healthy; // true if we can trust the altitude from the rangefinder
+        float alt;     // tilt compensated altitude from rangefinder
+        float min;     // min rangefinder distance
+        float max;     // max rangefinder distance
         uint32_t last_healthy_ms;
         float inertial_alt_cm;                  // inertial alt at time of last rangefinder sample
         float rangefinder_terrain_offset_cm;    // terrain height above EKF origin
-        LowPassFilterFloat alt_cm_filt;         // altitude filter
+        LowPassFilterFloat alt_filt;         // altitude filter
     } rangefinder_state = { false, false, 0, 0, 0, 0, 0, 0 };
 
 #if AP_RPM_ENABLED

ArduSub/mode_surftrak.cpp

@@ -67,9 +67,9 @@ bool ModeSurftrak::set_rangefinder_target_cm(float target_cm)
         sub.gcs().send_text(MAV_SEVERITY_WARNING, "wrong mode, rangefinder target not set");
     } else if (sub.inertial_nav.get_position_z_up_cm() >= sub.g.surftrak_depth) {
         sub.gcs().send_text(MAV_SEVERITY_WARNING, "descend below %f meters to set rangefinder target", sub.g.surftrak_depth * 0.01f);
-    } else if (target_cm < (float)sub.rangefinder_state.min_cm) {
+    } else if (target_cm < sub.rangefinder_state.min*100) {
         sub.gcs().send_text(MAV_SEVERITY_WARNING, "rangefinder target below minimum, ignored");
-    } else if (target_cm > (float)sub.rangefinder_state.max_cm) {
+    } else if (target_cm > sub.rangefinder_state.max*100) {
         sub.gcs().send_text(MAV_SEVERITY_WARNING, "rangefinder target above maximum, ignored");
     } else {
         success = true;

ArduSub/sensors.cpp

@@ -20,7 +20,7 @@ void Sub::init_rangefinder()
 #if AP_RANGEFINDER_ENABLED
     rangefinder.set_log_rfnd_bit(MASK_LOG_CTUN);
     rangefinder.init(ROTATION_PITCH_270);
-    rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ);
+    rangefinder_state.alt_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ);
     rangefinder_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_270);
 #endif
 }
@@ -39,30 +39,30 @@ void Sub::read_rangefinder()
             (rangefinder.range_valid_count_orient(ROTATION_PITCH_270) >= RANGEFINDER_HEALTH_MAX) &&
             (signal_quality_pct == -1 || signal_quality_pct >= g.rangefinder_signal_min);
 
-    int16_t temp_alt = rangefinder.distance_cm_orient(ROTATION_PITCH_270);
+    float temp_alt_m = rangefinder.distance_orient(ROTATION_PITCH_270);
 
 #if RANGEFINDER_TILT_CORRECTION
     // correct alt for angle of the rangefinder
-    temp_alt = (float)temp_alt * MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z);
+    temp_alt_m = temp_alt_m * MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z);
 #endif
 
-    rangefinder_state.alt_cm = temp_alt;
+    rangefinder_state.alt = temp_alt_m;
     rangefinder_state.inertial_alt_cm = inertial_nav.get_position_z_up_cm();
-    rangefinder_state.min_cm = rangefinder.min_distance_cm_orient(ROTATION_PITCH_270);
-    rangefinder_state.max_cm = rangefinder.max_distance_cm_orient(ROTATION_PITCH_270);
+    rangefinder_state.min = rangefinder.min_distance_orient(ROTATION_PITCH_270);
+    rangefinder_state.max = rangefinder.max_distance_orient(ROTATION_PITCH_270);
 
     // calculate rangefinder_terrain_offset_cm
     if (rangefinder_state.alt_healthy) {
         uint32_t now = AP_HAL::millis();
         if (now - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS) {
             // reset filter if we haven't used it within the last second
-            rangefinder_state.alt_cm_filt.reset(rangefinder_state.alt_cm);
+            rangefinder_state.alt_filt.reset(rangefinder_state.alt);
         } else {
-            rangefinder_state.alt_cm_filt.apply(rangefinder_state.alt_cm, 0.05f);
+            rangefinder_state.alt_filt.apply(rangefinder_state.alt, 0.05f);
         }
         rangefinder_state.last_healthy_ms = now;
         rangefinder_state.rangefinder_terrain_offset_cm =
-                sub.rangefinder_state.inertial_alt_cm - sub.rangefinder_state.alt_cm_filt.get();
+            sub.rangefinder_state.inertial_alt_cm - (sub.rangefinder_state.alt_filt.get() * 100);
     }
 
     // send rangefinder altitude and health to waypoint navigation library

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/examples/rewrite-RFND-values-to-floats.py

@@ -0,0 +1,132 @@
+#!/usr/bin/env python
+'''In ArduPilot 4.7 the distance values were moved to float to
+facilitate supporting rangefinders with more than 327m of range.
+
+The dataflash Replay log messages changes to support this.
+
+This script will take an older log and convert those messages such
+that older logs can be replayed with newer code.
+
+AP_FLAKE8_CLEAN
+'''
+
+from argparse import ArgumentParser
+
+import struct
+
+from pymavlink import DFReader
+from pymavlink import mavutil
+
+new_RRNH_format = "ffB"
+new_RRNI_format = "ffffBBB"
+
+
+class Rewrite():
+    def __init__(self, rewrite_fmt, rewrite_fmtu, rewrite_instance):
+        self.rewrite_fmt = rewrite_fmt
+        self.rewrite_fmtu = rewrite_fmtu
+        self.rewrite_instance = rewrite_instance
+
+    def format_to_struct(fmt):
+        ret = bytes("<", 'ascii')
+        for c in fmt:
+            (s, mul, type) = DFReader.FORMAT_TO_STRUCT[c]
+            ret += bytes(s, 'ascii')
+        return bytes(ret)
+
+
+rewrites = {
+    # convert RRNI.distance_cm > RRNI.distance
+    "RRNH": Rewrite(
+        rewrite_fmt=lambda buf, m : buf[:3] + struct.pack(
+            Rewrite.format_to_struct("BBnNZ"),
+            m.Type,
+            struct.calcsize(Rewrite.format_to_struct(new_RRNH_format)) + 3,  # m.Length
+            bytes(m.Name, 'ascii'),
+            bytes(new_RRNH_format, 'ascii'),
+            bytes(m.Columns, 'ascii')
+        ),
+        rewrite_fmtu=lambda buf, m : buf[:3] + struct.pack(
+            Rewrite.format_to_struct("QBNN"),
+            m.TimeUS,
+            m.FmtType,
+            bytes("mm-", 'ascii'),  # new units
+            bytes("00-", 'ascii')  # new mults
+        ),
+        rewrite_instance=lambda buf, m : buf[:3] + struct.pack(
+            Rewrite.format_to_struct(new_RRNH_format),
+            m.GCl * 0.01,
+            m.MaxD * 0.01,
+            m.NumSensors
+        ),
+    ),
+    "RRNI": Rewrite(
+        rewrite_fmt=lambda buf, m : buf[:3] + struct.pack(
+            Rewrite.format_to_struct("BBnNZ"),
+            m.Type,
+            struct.calcsize(Rewrite.format_to_struct(new_RRNI_format)) + 3,  # m.Length
+            bytes(m.Name, 'ascii'),
+            bytes(new_RRNI_format, 'ascii'),
+            bytes(m.Columns, 'ascii')
+        ),
+        rewrite_fmtu=lambda buf, m : buf[:3] + struct.pack(
+            Rewrite.format_to_struct("QBNN"),
+            m.TimeUS,
+            m.FmtType,
+            bytes("???m--#", 'ascii'),  # new units
+            bytes("???0---", 'ascii')   # new mults
+        ),
+        rewrite_instance=lambda buf, m : buf[:3] + struct.pack(
+            Rewrite.format_to_struct(new_RRNI_format),
+            m.PX,
+            m.PY,
+            m.PZ,
+            m.Dist * 0.01,
+            m.Orient,
+            m.Status,
+            m.I
+        ),
+    ),
+}
+
+
+parser = ArgumentParser(description=__doc__)
+
+parser.add_argument("login")
+parser.add_argument("logout")
+
+args = parser.parse_args()
+
+login = mavutil.mavlink_connection(args.login)
+output = open(args.logout, mode='wb')
+
+type_name_map = {}
+
+
+def rewrite_message(m):
+    buf = bytearray(m.get_msgbuf())
+
+    mtype = m.get_type()
+    if mtype == 'FMT':
+        type_name_map[m.Type] = m.Name
+        if m.Name in rewrites:
+            return rewrites[m.Name].rewrite_fmt(buf, m)
+
+    if mtype == 'FMTU':
+        if m.FmtType not in type_name_map:
+            raise ValueError(f"Have not seen format for ID {m.FmtType}")
+        name = type_name_map[m.FmtType]
+        if name in rewrites:
+            return rewrites[name].rewrite_fmtu(buf, m)
+
+    if mtype in rewrites:
+        return rewrites[mtype].rewrite_instance(buf, m)
+
+    return buf
+
+
+while True:
+    m = login.recv_msg()
+    if m is None:
+        break
+    output.write(rewrite_message(m))