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

AP_RangeFinder: correct MAVLink backend's out-of-range checks

This commit is contained in:
Peter Barker 2020-12-05 17:49:18 +11:00 committed by Peter Barker
parent 7474948971
commit cba5f142a7
4 changed files with 13 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp
View File

@ -56,9 +56,9 @@ bool AP_RangeFinder_Backend::has_data() const {
void AP_RangeFinder_Backend::update_status()
{
// check distance
if ((int16_t)state.distance_cm > params.max_distance_cm) {
if ((int16_t)state.distance_cm > max_distance_cm()) {
set_status(RangeFinder::Status::OutOfRangeHigh);
} else if ((int16_t)state.distance_cm < params.min_distance_cm) {
} else if ((int16_t)state.distance_cm < min_distance_cm()) {
set_status(RangeFinder::Status::OutOfRangeLow);
} else {
set_status(RangeFinder::Status::Good);

4
libraries/AP_RangeFinder/AP_RangeFinder_Backend.h
View File

@ -39,8 +39,8 @@ public:
enum Rotation orientation() const { return (Rotation)params.orientation.get(); }
uint16_t distance_cm() const { return state.distance_cm; }
uint16_t voltage_mv() const { return state.voltage_mv; }
int16_t max_distance_cm() const { return params.max_distance_cm; }
int16_t min_distance_cm() const { return params.min_distance_cm; }
virtual int16_t max_distance_cm() const { return params.max_distance_cm; }
virtual int16_t min_distance_cm() const { return params.min_distance_cm; }
int16_t ground_clearance_cm() const { return params.ground_clearance_cm; }
MAV_DISTANCE_SENSOR get_mav_distance_sensor_type() const;
RangeFinder::Status status() const;

2
libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp
View File

@ -28,6 +28,8 @@ void AP_RangeFinder_MAVLink::handle_msg(const mavlink_message_t &msg)
if (packet.orientation == MAV_SENSOR_ROTATION_PITCH_270) {
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;
}
}

7
libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h
View File

@ -23,6 +23,9 @@ public:
// Get update from mavlink
void handle_msg(const mavlink_message_t &msg) override;
int16_t max_distance_cm() const override { return _max_distance_cm; }
int16_t min_distance_cm() const override { return _min_distance_cm; }
protected:
MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
@ -30,7 +33,11 @@ protected:
}
private:
// stored data from packet:
uint16_t distance_cm;
uint16_t _max_distance_cm;
uint16_t _min_distance_cm;
// start a reading
static bool start_reading(void);