diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp index 738e28d134..c0c78871b8 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp @@ -34,6 +34,30 @@ void AP_RangeFinder_MAVLink::handle_msg(const mavlink_message_t &msg) } } +int16_t AP_RangeFinder_MAVLink::max_distance_cm() const +{ + if (_max_distance_cm == 0 && _min_distance_cm == 0) { + // we assume if both of these are zero that we ignore both + return params.max_distance_cm; + } + + if (params.max_distance_cm < _max_distance_cm) { + return params.max_distance_cm; + } + return _max_distance_cm; +} +int16_t AP_RangeFinder_MAVLink::min_distance_cm() const +{ + if (_max_distance_cm == 0 && _min_distance_cm == 0) { + // we assume if both of these are zero that we ignore both + return params.min_distance_cm; + } + if (params.min_distance_cm > _min_distance_cm) { + return params.min_distance_cm; + } + return _min_distance_cm; +} + /* update the state of the sensor */ diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h index a845e43e27..ba39e1f5af 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h @@ -23,8 +23,8 @@ class AP_RangeFinder_MAVLink : public AP_RangeFinder_Backend // 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; } + int16_t max_distance_cm() const override; + int16_t min_distance_cm() const override; protected: