diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index 1d21089df5acc..b9ea6f90765ed 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/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)); } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index b00198644fa8e..49230664c269b 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -193,9 +193,14 @@ class RangeFinder 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 @@ class RangeFinder // 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; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp index 42e64add72e82..3cd40aa298e73 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp +++ b/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 true; + return protocol.get_confidence(); } uint8_t PingProtocol::get_confidence() const diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h index e61386bf4249e..04fccaae4e35e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h @@ -138,7 +138,7 @@ class AP_RangeFinder_BLPing : public AP_RangeFinder_Backend_Serial * 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: /** diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h index 5ae14f0d5b96d..86f487364ba34 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h @@ -48,6 +48,7 @@ class AP_RangeFinder_Backend 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 @@ class AP_RangeFinder_Backend // 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; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.cpp index 186e4d1bd793a..af27aaf5a7ad4 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.cpp +++ b/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(); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.h index ecb7508d476cf..e4349d75b8a8f 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.h @@ -29,6 +29,12 @@ class AP_RangeFinder_Backend_Serial : public AP_RangeFinder_Backend // 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; } }; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h index df9fd1b00cc64..e37030a971398 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h @@ -82,9 +82,9 @@ class AP_RangeFinder_JRE_Serial : public AP_RangeFinder_Backend_Serial return MAV_DISTANCE_SENSOR_RADAR; } - bool get_signal_quality_pct(int8_t &quality_pct) const override { - quality_pct = no_signal ? 0 : 100; - return true; + int8_t get_signal_quality_pct() const override + { + return no_signal ? RangeFinder::SIGNAL_QUALITY_MIN : RangeFinder::SIGNAL_QUALITY_MAX; } private: diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h index cbdfe5d6a52f2..13f4e776cdd0e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h @@ -26,9 +26,8 @@ class AP_RangeFinder_LightWareSerial : public AP_RangeFinder_Backend_Serial return MAV_DISTANCE_SENSOR_LASER; } - bool get_signal_quality_pct(int8_t &quality_pct) const override { - quality_pct = no_signal ? 0 : 100; - return true; + int8_t get_signal_quality_pct() const override { + return no_signal ? RangeFinder::SIGNAL_QUALITY_MIN : RangeFinder::SIGNAL_QUALITY_MAX; } private: diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp index 66ad7aebada8f..b4fc5fb1ff597 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp +++ b/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 diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h index e2140260d1907..47cd080c13f9a 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h @@ -31,10 +31,6 @@ class AP_RangeFinder_MAVLink : public AP_RangeFinder_Backend 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 { diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index f501f7d4657d5..16b4d43d83a9b 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/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(); + // ardupilot defines this field as -1 is unknown, 0 is poor, 100 is excellent + // mavlink defines this field as 0 is unknown, 1 is invalid, 100 is perfect uint8_t quality; - 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); - } 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(