mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
RangeFinder_MaxSonar: set status
This commit is contained in:
parent
e212ebb0d0
commit
fadfa70e99
@ -116,5 +116,10 @@ bool AP_RangeFinder_MaxsonarI2CXL::get_reading(uint16_t &reading_cm)
|
||||
*/
|
||||
void AP_RangeFinder_MaxsonarI2CXL::update(void)
|
||||
{
|
||||
state.healthy = get_reading(state.distance_cm);
|
||||
if (get_reading(state.distance_cm)) {
|
||||
// update range_valid state based on distance measured
|
||||
update_status();
|
||||
} else {
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
}
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user