Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Rangefinder: probe all I2C buses for rangefinders

This commit is contained in:
Andrew Tridgell 2019-02-12 15:29:39 +11:00 committed by Randy Mackay
parent 2f40967820
commit 481a6ea570
1 changed files with 24 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -644,15 +644,18 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
case RangeFinder_TYPE_PLI2C: case RangeFinder_TYPE_PLI2C:
case RangeFinder_TYPE_PLI2CV3: case RangeFinder_TYPE_PLI2CV3:
case RangeFinder_TYPE_PLI2CV3HP: case RangeFinder_TYPE_PLI2CV3HP:
if (!_add_backend(AP_RangeFinder_PulsedLightLRF::detect(1, state[instance], _type))) { for (int8_t i=3; i>=0; i--) {
_add_backend(AP_RangeFinder_PulsedLightLRF::detect(0, state[instance], _type)); if (_add_backend(AP_RangeFinder_PulsedLightLRF::detect(i, state[instance], _type))) {
break;
}
} }
break; break;
case RangeFinder_TYPE_MBI2C: case RangeFinder_TYPE_MBI2C:
if (!_add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(state[instance], for (int8_t i=3; i>=0; i--) {
hal.i2c_mgr->get_device(1, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR)))) { if (_add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(state[instance],
_add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(state[instance], hal.i2c_mgr->get_device(i, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR)))) {
hal.i2c_mgr->get_device(0, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR))); break;
}
} }
break; break;
case RangeFinder_TYPE_LWI2C: case RangeFinder_TYPE_LWI2C:
@ -661,28 +664,31 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], _add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance],
hal.i2c_mgr->get_device(HAL_RANGEFINDER_LIGHTWARE_I2C_BUS, state[instance].address))); hal.i2c_mgr->get_device(HAL_RANGEFINDER_LIGHTWARE_I2C_BUS, state[instance].address)));
#else #else
if (!_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], for (int8_t i=3; i>=0; i--) {
hal.i2c_mgr->get_device(1, state[instance].address)))) { if (_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance],
_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], hal.i2c_mgr->get_device(i, state[instance].address)))) {
hal.i2c_mgr->get_device(0, state[instance].address))); break;
}
} }
#endif #endif
} }
break; break;
case RangeFinder_TYPE_TRI2C: case RangeFinder_TYPE_TRI2C:
if (state[instance].address) { if (state[instance].address) {
if (!_add_backend(AP_RangeFinder_TeraRangerI2C::detect(state[instance], for (int8_t i=3; i>=0; i--) {
hal.i2c_mgr->get_device(1, state[instance].address)))) { if (_add_backend(AP_RangeFinder_TeraRangerI2C::detect(state[instance],
_add_backend(AP_RangeFinder_TeraRangerI2C::detect(state[instance], hal.i2c_mgr->get_device(i, state[instance].address)))) {
hal.i2c_mgr->get_device(0, state[instance].address))); break;
}
} }
} }
break; break;
case RangeFinder_TYPE_VL53L0X: case RangeFinder_TYPE_VL53L0X:
if (!_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance], for (int8_t i=3; i>=0; i--) {
hal.i2c_mgr->get_device(1, 0x29)))) { if (_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance],
_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance], hal.i2c_mgr->get_device(i, 0x29)))) {
hal.i2c_mgr->get_device(0, 0x29))); break;
}
} }
break; break;
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4