diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp index 7836fc2f2e..17ccb035a5 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp @@ -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); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h index c6c7c6d214..7be7ff8257 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h @@ -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; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp index c010258f30..738e28d134 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp @@ -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; } } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h index 52728b3a7c..a845e43e27 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h @@ -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);