AP_RangeFinder: Add signal_quality_pct to range finder state

libraries/AP_RangeFinder/AP_RangeFinder.cpp

@@ -220,6 +220,8 @@ void RangeFinder::init(enum Rotation orientation_default)
         // initialise status
         state[i].status = Status::NotConnected;
         state[i].range_valid_count = 0;
+        // initialize signal_quality_pct for drivers that don't handle it.
+        state[i].signal_quality_pct = SIGNAL_QUALITY_UNKNOWN;
     }
 }
 
@@ -693,6 +695,15 @@ uint16_t RangeFinder::distance_cm_orient(enum Rotation orientation) const
     return distance_orient(orientation) * 100.0;
 }
 
+int8_t RangeFinder::signal_quality_pct_orient(enum Rotation orientation) const
+{
+    AP_RangeFinder_Backend *backend = find_instance(orientation);
+    if (backend == nullptr) {
+        return RangeFinder::SIGNAL_QUALITY_UNKNOWN;
+    }
+    return backend->signal_quality_pct();
+}
+
 int16_t RangeFinder::max_distance_cm_orient(enum Rotation orientation) const
 {
     AP_RangeFinder_Backend *backend = find_instance(orientation);
@@ -793,10 +804,6 @@ void RangeFinder::Log_RFND() const
             continue;
         }
 
-        int8_t signal_quality;
-        if (!s->get_signal_quality_pct(signal_quality)) {
-          signal_quality = -1;
-        }
         const struct log_RFND pkt = {
                 LOG_PACKET_HEADER_INIT(LOG_RFND_MSG),
                 time_us      : AP_HAL::micros64(),
@@ -804,7 +811,7 @@ void RangeFinder::Log_RFND() const
                 dist         : s->distance_cm(),
                 status       : (uint8_t)s->status(),
                 orient       : s->orientation(),
-                quality      : signal_quality,
+                quality      : s->signal_quality_pct(),
         };
         AP::logger().WriteBlock(&pkt, sizeof(pkt));
     }

libraries/AP_RangeFinder/AP_RangeFinder.h

@@ -193,9 +193,14 @@ public:
         Good
     };
 
+    static constexpr int8_t SIGNAL_QUALITY_MIN = 0;
+    static constexpr int8_t SIGNAL_QUALITY_MAX = 100;
+    static constexpr int8_t SIGNAL_QUALITY_UNKNOWN = -1;
+
     // The RangeFinder_State structure is filled in by the backend driver
     struct RangeFinder_State {
         float distance_m;               // distance in meters
+        int8_t signal_quality_pct;      // measurement quality in percent 0-100, -1 -> quality is unknown
         uint16_t voltage_mv;            // voltage in millivolts, if applicable, otherwise 0
         enum RangeFinder::Status status; // sensor status
         uint8_t  range_valid_count;     // number of consecutive valid readings (maxes out at 10)
@@ -260,6 +265,7 @@ public:
     // any sensor which can current supply it
     float distance_orient(enum Rotation orientation) const;
     uint16_t distance_cm_orient(enum Rotation orientation) const;
+    int8_t signal_quality_pct_orient(enum Rotation orientation) const;
     int16_t max_distance_cm_orient(enum Rotation orientation) const;
     int16_t min_distance_cm_orient(enum Rotation orientation) const;
     int16_t ground_clearance_cm_orient(enum Rotation orientation) const;

libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp

@@ -83,13 +83,12 @@ bool AP_RangeFinder_BLPing::get_reading(float &reading_m)
     return false;
 }
 
-bool AP_RangeFinder_BLPing::get_signal_quality_pct(int8_t &quality_pct) const
+int8_t AP_RangeFinder_BLPing::get_signal_quality_pct() const
 {
     if (status() != RangeFinder::Status::Good) {
-        return false;
+        return RangeFinder::SIGNAL_QUALITY_UNKNOWN;
     }
-    quality_pct = protocol.get_confidence();
+    return protocol.get_confidence();
-    return true;
 }
 
 uint8_t PingProtocol::get_confidence() const

libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h

@@ -138,7 +138,7 @@ public:
      * 100 is best quality, 0 is worst
      *
      */
-    bool get_signal_quality_pct(int8_t &quality_pct) const override WARN_IF_UNUSED;
+    int8_t get_signal_quality_pct() const override WARN_IF_UNUSED;
 
 protected:
     /**

libraries/AP_RangeFinder/AP_RangeFinder_Backend.h

@@ -48,6 +48,7 @@ public:
     enum Rotation orientation() const { return (Rotation)params.orientation.get(); }
     float distance() const { return state.distance_m; }
     uint16_t distance_cm() const { return state.distance_m*100.0f; }
+    int8_t signal_quality_pct() const  WARN_IF_UNUSED { return state.signal_quality_pct; }
     uint16_t voltage_mv() const { return state.voltage_mv; }
     virtual int16_t max_distance_cm() const { return params.max_distance_cm; }
     virtual int16_t min_distance_cm() const { return params.min_distance_cm; }
@@ -72,10 +73,6 @@ public:
     // get temperature reading in C.  returns true on success and populates temp argument
     virtual bool get_temp(float &temp) const { return false; }
 
-    // 0 is no return value, 100 is perfect.  false means signal
-    // quality is not available
-    virtual bool get_signal_quality_pct(int8_t &quality_pct) const { return false; }
-
     // return the actual type of the rangefinder, as opposed to the
     // parameter value which may be changed at runtime.
     RangeFinder::Type allocated_type() const { return _backend_type; }

libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.cpp

@@ -53,6 +53,7 @@ uint32_t AP_RangeFinder_Backend_Serial::initial_baudrate(const uint8_t serial_in
 void AP_RangeFinder_Backend_Serial::update(void)
 {
     if (get_reading(state.distance_m)) {
+        state.signal_quality_pct = get_signal_quality_pct();
         // update range_valid state based on distance measured
         state.last_reading_ms = AP_HAL::millis();
         update_status();

libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.h

@@ -29,6 +29,12 @@ protected:
     // implement this:
     virtual bool get_reading(float &reading_m) = 0;
 
+    // returns 0-100 or -1. This virtual method is for
+    // serial drivers and is a companion to the previous method get_reading().
+    // Like get_reading() this method is called in the base-class update() method.
+    virtual int8_t get_signal_quality_pct() const WARN_IF_UNUSED
+    { return RangeFinder::SIGNAL_QUALITY_UNKNOWN; }
+
     // maximum time between readings before we change state to NoData:
     virtual uint16_t read_timeout_ms() const { return 200; }
 };

libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h

@@ -82,9 +82,9 @@ protected:
         return MAV_DISTANCE_SENSOR_RADAR;
     }
 
-    bool get_signal_quality_pct(int8_t &quality_pct) const override {
+    int8_t get_signal_quality_pct() const override
-        quality_pct = no_signal ? 0 : 100;
+    {
-        return true;
+        return no_signal ? RangeFinder::SIGNAL_QUALITY_MIN : RangeFinder::SIGNAL_QUALITY_MAX;
     }
 
 private:

libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h

@@ -26,9 +26,8 @@ protected:
         return MAV_DISTANCE_SENSOR_LASER;
     }
 
-    bool get_signal_quality_pct(int8_t &quality_pct) const override {
+    int8_t get_signal_quality_pct() const override {
-        quality_pct = no_signal ? 0 : 100;
+        return no_signal ? RangeFinder::SIGNAL_QUALITY_MIN : RangeFinder::SIGNAL_QUALITY_MAX;
-        return true;
     }
 
 private:

libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp

@@ -37,10 +37,10 @@ void AP_RangeFinder_MAVLink::handle_msg(const mavlink_message_t &msg)
         signal_quality = packet.signal_quality;
         if (signal_quality == 0) {
             // MAVLink's 0 means invalid/unset, so we map it to -1
-            signal_quality = -1;
+            signal_quality = RangeFinder::SIGNAL_QUALITY_UNKNOWN;
         } else if (signal_quality == 1) {
             // Map 1 to 0 as that is what ardupilot uses as the worst signal quality
-            signal_quality = 0;
+            signal_quality = RangeFinder::SIGNAL_QUALITY_MIN;
         }
     }
 }
@@ -79,19 +79,12 @@ void AP_RangeFinder_MAVLink::update(void)
     if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_MAVLINK_TIMEOUT_MS) {
         set_status(RangeFinder::Status::NoData);
         state.distance_m = 0.0f;
+        state.signal_quality_pct = RangeFinder::SIGNAL_QUALITY_UNKNOWN;
     } else {
         state.distance_m = distance_cm * 0.01f;
+        state.signal_quality_pct = signal_quality;
         update_status();
     }
 }
 
-bool AP_RangeFinder_MAVLink::get_signal_quality_pct(int8_t &quality_pct) const
-{
-    if (status() != RangeFinder::Status::Good) {
-        return false;
-    }
-    quality_pct = signal_quality;
-    return true;
-}
-
 #endif

libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h

@@ -31,10 +31,6 @@ public:
     int16_t max_distance_cm() const override;
     int16_t min_distance_cm() const override;
 
-    // Get the reading confidence
-    // 100 is best quality, 0 is worst
-    WARN_IF_UNUSED bool get_signal_quality_pct(int8_t &quality_pct) const override;
-
 protected:
 
     MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -385,13 +385,16 @@ void GCS_MAVLINK::send_distance_sensor(const AP_RangeFinder_Backend *sensor, con
         return;
     }
 
-    int8_t quality_pct;
+    int8_t quality_pct = sensor->signal_quality_pct();
-    uint8_t quality;
+    // ardupilot defines this field as -1 is unknown, 0 is poor, 100 is excellent
-    if (sensor->get_signal_quality_pct(quality_pct)) {
     // mavlink defines this field as 0 is unknown, 1 is invalid, 100 is perfect
-        quality = MAX(quality_pct, 1);
+    uint8_t quality;
-    } else {
+    if (quality_pct == RangeFinder::SIGNAL_QUALITY_UNKNOWN) {
         quality = 0;
+    } else if (quality_pct > 1 && quality_pct <= RangeFinder::SIGNAL_QUALITY_MAX) {
+        quality = quality_pct;
+    } else {
+        quality = 1;
     }
 
     mavlink_msg_distance_sensor_send(