diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp index d8fca667a8..8261849520 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp @@ -133,6 +133,7 @@ void AP_RangeFinder_MaxsonarI2CXL::_timer(void) if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { distance = d; new_distance = true; + last_update_ms = AP_HAL::millis(); _sem->give(); } } @@ -148,9 +149,10 @@ void AP_RangeFinder_MaxsonarI2CXL::update(void) state.distance_cm = distance; new_distance = false; update_status(); - } else { + } else if (AP_HAL::millis() - last_update_ms > 300) { + // if no updates for 0.3 seconds set no-data set_status(RangeFinder::RangeFinder_NoData); } - _sem->give(); + _sem->give(); } } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h index dc34d0bd27..ae721c60da 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h @@ -39,6 +39,7 @@ private: uint16_t distance; bool new_distance; + uint32_t last_update_ms; // start a reading bool start_reading(void);