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

AP_RangeFinder_MAVLink: implement get_signal_quality_pct 

Co-authored-by:  Willian Galvani <williangalvani@gmail.com>
This commit is contained in:
Clyde McQueen 2023-08-14 13:21:56 -07:00 committed by Peter Barker
parent 494b3076ac
commit dfe6c21ea5
2 changed files with 23 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp
View File

@ -27,13 +27,21 @@ void AP_RangeFinder_MAVLink::handle_msg(const mavlink_message_t &msg)
mavlink_distance_sensor_t packet;
mavlink_msg_distance_sensor_decode(&msg, &packet);
// only accept distances for the configured orentation
// only accept distances for the configured orientation
if (packet.orientation == orientation()) {
state.last_reading_ms = AP_HAL::millis();
distance_cm = packet.current_distance;
_max_distance_cm = packet.max_distance;
_min_distance_cm = packet.min_distance;
sensor_type = (MAV_DISTANCE_SENSOR)packet.type;
signal_quality = packet.signal_quality;
if (signal_quality == 0) {
// MAVLink's 0 means invalid/unset, so we map it to -1
signal_quality = -1;
} else if (signal_quality == 1) {
// Map 1 to 0 as that is what ardupilot uses as the worst signal quality
signal_quality = 0;
}
}
}
@ -77,4 +85,13 @@ void AP_RangeFinder_MAVLink::update(void)
}
}
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

5
libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h
View File

@ -31,6 +31,10 @@ 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 {
@ -43,6 +47,7 @@ private:
uint16_t distance_cm;
uint16_t _max_distance_cm;
uint16_t _min_distance_cm;
int8_t signal_quality;
// start a reading
static bool start_reading(void);