aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
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() void AP_RangeFinder_Backend::update_status()
{ {
// check distance // 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); 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); set_status(RangeFinder::Status::OutOfRangeLow);
} else { } else {
set_status(RangeFinder::Status::Good); 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(); } enum Rotation orientation() const { return (Rotation)params.orientation.get(); }
uint16_t distance_cm() const { return state.distance_cm; } uint16_t distance_cm() const { return state.distance_cm; }
uint16_t voltage_mv() const { return state.voltage_mv; } uint16_t voltage_mv() const { return state.voltage_mv; }
int16_t max_distance_cm() const { return params.max_distance_cm; } virtual 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 min_distance_cm() const { return params.min_distance_cm; }
int16_t ground_clearance_cm() const { return params.ground_clearance_cm; } int16_t ground_clearance_cm() const { return params.ground_clearance_cm; }
MAV_DISTANCE_SENSOR get_mav_distance_sensor_type() const; MAV_DISTANCE_SENSOR get_mav_distance_sensor_type() const;
RangeFinder::Status status() 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) { if (packet.orientation == MAV_SENSOR_ROTATION_PITCH_270) {
state.last_reading_ms = AP_HAL::millis(); state.last_reading_ms = AP_HAL::millis();
distance_cm = packet.current_distance; distance_cm = packet.current_distance;
_max_distance_cm = packet.max_distance;
_min_distance_cm = packet.min_distance;
sensor_type = (MAV_DISTANCE_SENSOR)packet.type; 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 // Get update from mavlink
void handle_msg(const mavlink_message_t &msg) override; 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: protected:
MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
@ -30,7 +33,11 @@ protected:
} }
private: private:
// stored data from packet:
uint16_t distance_cm; uint16_t distance_cm;
uint16_t _max_distance_cm;
uint16_t _min_distance_cm;
// start a reading // start a reading
static bool start_reading(void); static bool start_reading(void);