Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: Add signal_quality_pct to range finder state

This commit is contained in:
Peter Mullen 2023-11-14 20:07:34 -08:00 committed by Peter Barker
parent 8eb5baa4ae
commit adc0ebf9de
12 changed files with 47 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_RangeFinder/AP_RangeFinder.cpp
View File

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

6
libraries/AP_RangeFinder/AP_RangeFinder.h
View File

@ -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;

7
libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp
View File

@ -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

2
libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h
View File

@ -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:
/**

5
libraries/AP_RangeFinder/AP_RangeFinder_Backend.h
View File

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

1
libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.cpp
View File

@ -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();

6
libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.h
View File

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

6
libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h
View File

@ -82,9 +82,9 @@ protected:
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:

5
libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h
View File

@ -26,9 +26,8 @@ protected:
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:

15
libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp
View File

@ -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

4
libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h
View File

@ -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 {

13
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -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(