RangeFinder_PulsedLight: set status

This commit is contained in:
Randy Mackay 2015-04-13 15:07:32 +09:00
parent aa04bef5af
commit e212ebb0d0
1 changed files with 6 additions and 1 deletions

View File

@ -109,5 +109,10 @@ bool AP_RangeFinder_PulsedLightLRF::get_reading(uint16_t &reading_cm)
*/
void AP_RangeFinder_PulsedLightLRF::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);
}
}