AP_RangeFinder: try bus 0 and 1 for PulsedLightI2C rangefinder

This commit is contained in:
Andrew Tridgell 2016-11-11 13:19:09 +11:00
parent 516141edf3
commit a69ff34c80

View File

@ -29,8 +29,13 @@ extern const AP_HAL::HAL& hal;
AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(RangeFinder &_ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state)
: AP_RangeFinder_Backend(_ranger, instance, _state)
, _dev(hal.i2c_mgr->get_device(1, AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR))
{
// try external bus first
_dev = hal.i2c_mgr->get_device(1, AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR);
if (!_dev) {
// then internal
_dev = hal.i2c_mgr->get_device(0, AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR);
}
}
/*