diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp index ae620f8bd8..cc29868a4c 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp @@ -39,7 +39,9 @@ uint8_t AP_RangeFinder_PX4::num_px4_instances = 0; already know that we should setup the rangefinder */ AP_RangeFinder_PX4::AP_RangeFinder_PX4(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) : - AP_RangeFinder_Backend(_ranger, instance, _state) + AP_RangeFinder_Backend(_ranger, instance, _state), + _last_max_distance_cm(-1), + _last_min_distance_cm(-1) { _fd = open_driver(); @@ -111,6 +113,18 @@ void AP_RangeFinder_PX4::update(void) float sum = 0; uint16_t count = 0; + if (_last_max_distance_cm != ranger._max_distance_cm[state.instance] || + _last_min_distance_cm != ranger._min_distance_cm[state.instance]) { + float max_distance = ranger._max_distance_cm[state.instance]*0.01f; + float min_distance = ranger._min_distance_cm[state.instance]*0.01f; + if (ioctl(_fd, RANGEFINDERIOCSETMAXIUMDISTANCE, (unsigned long)&max_distance) == 0 && + ioctl(_fd, RANGEFINDERIOCSETMINIUMDISTANCE, (unsigned long)&min_distance) == 0) { + _last_max_distance_cm = ranger._max_distance_cm[state.instance]; + _last_min_distance_cm = ranger._min_distance_cm[state.instance]; + } + } + + while (::read(_fd, &range_report, sizeof(range_report)) == sizeof(range_report) && range_report.timestamp != _last_timestamp) { // Only take valid readings diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h index f096429c92..48d5fb202e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h @@ -39,6 +39,9 @@ private: int _fd; uint64_t _last_timestamp; + int16_t _last_max_distance_cm; + int16_t _last_min_distance_cm; + // we need to keep track of how many PX4 drivers have been loaded // so we can open the right device filename static uint8_t num_px4_instances;