Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: fixed handling of instance numbers

Browse Source 
if a backend allocation failed then we could end up with the backend
being assigned to the wrong instance number. Switch to using
_add_backend() everywhere, and pass the instance number to
_add_backend() to ensure that it always uses the correct slot.

Also added comment explaining how num_sensors() works when we have a
sparse set of rangefinders configured
This commit is contained in:
Andrew Tridgell 2020-11-27 08:51:38 +11:00
parent 73755e2d1f
commit 0c05e9a2e7
2 changed files with 54 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
libraries/AP_RangeFinder/AP_RangeFinder.cpp
View File

@ -313,16 +313,22 @@ void RangeFinder::update(void)
#endif #endif
} }
bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend) bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend, uint8_t instance)
{ {
if (!backend) { if (!backend) {
return false; return false;
} }
if (num_instances == RANGEFINDER_MAX_INSTANCES) { if (instance >= RANGEFINDER_MAX_INSTANCES) {
AP_HAL::panic("Too many RANGERS backends"); AP_HAL::panic("Too many RANGERS backends");
} }
if (drivers[instance] != nullptr) {
// we've allocated the same instance twice
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
drivers[instance] = backend;
num_instances = MAX(num_instances, instance+1);
drivers[num_instances++] = backend;
return true; return true;
} }
@ -337,7 +343,8 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
case Type::PLI2CV3: case Type::PLI2CV3:
case Type::PLI2CV3HP: case Type::PLI2CV3HP:
FOREACH_I2C(i) { FOREACH_I2C(i) {
if (_add_backend(AP_RangeFinder_PulsedLightLRF::detect(i, state[instance], params[instance], _type))) { if (_add_backend(AP_RangeFinder_PulsedLightLRF::detect(i, state[instance], params[instance], _type),
instance)) {
break; break;
} }
} }
@ -345,7 +352,8 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
case Type::MBI2C: case Type::MBI2C:
FOREACH_I2C(i) { FOREACH_I2C(i) {
if (_add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(state[instance], params[instance], if (_add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(state[instance], params[instance],
hal.i2c_mgr->get_device(i, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR)))) { hal.i2c_mgr->get_device(i, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR)),
instance)) {
break; break;
} }
} }
@ -358,11 +366,13 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
} }
#ifdef HAL_RANGEFINDER_LIGHTWARE_I2C_BUS #ifdef HAL_RANGEFINDER_LIGHTWARE_I2C_BUS
_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], params[instance], _add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], params[instance],
hal.i2c_mgr->get_device(HAL_RANGEFINDER_LIGHTWARE_I2C_BUS, params[instance].address))); hal.i2c_mgr->get_device(HAL_RANGEFINDER_LIGHTWARE_I2C_BUS, params[instance].address)),
instance);
#else #else
FOREACH_I2C(i) { FOREACH_I2C(i) {
if (_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], params[instance], if (_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], params[instance],
hal.i2c_mgr->get_device(i, params[instance].address)))) { hal.i2c_mgr->get_device(i, params[instance].address)),
instance)) {
break; break;
} }
} }
@ -373,7 +383,8 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
if (params[instance].address) { if (params[instance].address) {
FOREACH_I2C(i) { FOREACH_I2C(i) {
if (_add_backend(AP_RangeFinder_TeraRangerI2C::detect(state[instance], params[instance], if (_add_backend(AP_RangeFinder_TeraRangerI2C::detect(state[instance], params[instance],
hal.i2c_mgr->get_device(i, params[instance].address)))) { hal.i2c_mgr->get_device(i, params[instance].address)),
instance)) {
break; break;
} }
} }
@ -383,13 +394,15 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
case Type::VL53L1X_Short: case Type::VL53L1X_Short:
FOREACH_I2C(i) { FOREACH_I2C(i) {
if (_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance], params[instance], if (_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance], params[instance],
hal.i2c_mgr->get_device(i, params[instance].address)))) { hal.i2c_mgr->get_device(i, params[instance].address)),
instance)) {
break; break;
} }
if (_add_backend(AP_RangeFinder_VL53L1X::detect(state[instance], params[instance], if (_add_backend(AP_RangeFinder_VL53L1X::detect(state[instance], params[instance],
hal.i2c_mgr->get_device(i, params[instance].address), hal.i2c_mgr->get_device(i, params[instance].address),
_type == Type::VL53L1X_Short ? AP_RangeFinder_VL53L1X::DistanceMode::Short : _type == Type::VL53L1X_Short ? AP_RangeFinder_VL53L1X::DistanceMode::Short :
AP_RangeFinder_VL53L1X::DistanceMode::Long))) { AP_RangeFinder_VL53L1X::DistanceMode::Long),
instance)) {
break; break;
} }
} }
@ -397,7 +410,8 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
case Type::BenewakeTFminiPlus: case Type::BenewakeTFminiPlus:
FOREACH_I2C(i) { FOREACH_I2C(i) {
if (_add_backend(AP_RangeFinder_Benewake_TFMiniPlus::detect(state[instance], params[instance], if (_add_backend(AP_RangeFinder_Benewake_TFMiniPlus::detect(state[instance], params[instance],
hal.i2c_mgr->get_device(i, params[instance].address)))) { hal.i2c_mgr->get_device(i, params[instance].address)),
instance)) {
break; break;
} }
} }
@ -408,7 +422,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
// to ease moving from PX4 to ChibiOS we'll lie a little about // to ease moving from PX4 to ChibiOS we'll lie a little about
// the backend driver... // the backend driver...
if (AP_RangeFinder_PWM::detect()) { if (AP_RangeFinder_PWM::detect()) {
drivers[instance] = new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height); _add_backend(new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height), instance);
} }
#endif #endif
#endif #endif
@ -416,50 +430,50 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
case Type::BBB_PRU: case Type::BBB_PRU:
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
if (AP_RangeFinder_BBB_PRU::detect()) { if (AP_RangeFinder_BBB_PRU::detect()) {
drivers[instance] = new AP_RangeFinder_BBB_PRU(state[instance], params[instance]); _add_backend(new AP_RangeFinder_BBB_PRU(state[instance], params[instance]), instance);
} }
#endif #endif
break; break;
case Type::LWSER: case Type::LWSER:
if (AP_RangeFinder_LightWareSerial::detect(serial_instance)) { if (AP_RangeFinder_LightWareSerial::detect(serial_instance)) {
drivers[instance] = new AP_RangeFinder_LightWareSerial(state[instance], params[instance], serial_instance++); _add_backend(new AP_RangeFinder_LightWareSerial(state[instance], params[instance], serial_instance++), instance);
} }
break; break;
case Type::LEDDARONE: case Type::LEDDARONE:
if (AP_RangeFinder_LeddarOne::detect(serial_instance)) { if (AP_RangeFinder_LeddarOne::detect(serial_instance)) {
drivers[instance] = new AP_RangeFinder_LeddarOne(state[instance], params[instance], serial_instance++); _add_backend(new AP_RangeFinder_LeddarOne(state[instance], params[instance], serial_instance++), instance);
} }
break; break;
case Type::ULANDING: case Type::ULANDING:
if (AP_RangeFinder_uLanding::detect(serial_instance)) { if (AP_RangeFinder_uLanding::detect(serial_instance)) {
drivers[instance] = new AP_RangeFinder_uLanding(state[instance], params[instance], serial_instance++); _add_backend(new AP_RangeFinder_uLanding(state[instance], params[instance], serial_instance++), instance);
} }
break; break;
case Type::BEBOP: case Type::BEBOP:
#if (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || \ #if (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO) && defined(HAVE_LIBIIO) CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO) && defined(HAVE_LIBIIO)
if (AP_RangeFinder_Bebop::detect()) { if (AP_RangeFinder_Bebop::detect()) {
drivers[instance] = new AP_RangeFinder_Bebop(state[instance], params[instance]); _add_backend(new AP_RangeFinder_Bebop(state[instance], params[instance]), instance);
} }
#endif #endif
break; break;
case Type::MAVLink: case Type::MAVLink:
#ifndef HAL_BUILD_AP_PERIPH #ifndef HAL_BUILD_AP_PERIPH
if (AP_RangeFinder_MAVLink::detect()) { if (AP_RangeFinder_MAVLink::detect()) {
drivers[instance] = new AP_RangeFinder_MAVLink(state[instance], params[instance]); _add_backend(new AP_RangeFinder_MAVLink(state[instance], params[instance]), instance);
} }
#endif #endif
break; break;
case Type::MBSER: case Type::MBSER:
if (AP_RangeFinder_MaxsonarSerialLV::detect(serial_instance)) { if (AP_RangeFinder_MaxsonarSerialLV::detect(serial_instance)) {
drivers[instance] = new AP_RangeFinder_MaxsonarSerialLV(state[instance], params[instance], serial_instance++); _add_backend(new AP_RangeFinder_MaxsonarSerialLV(state[instance], params[instance], serial_instance++), instance);
} }
break; break;
case Type::ANALOG: case Type::ANALOG:
#ifndef HAL_BUILD_AP_PERIPH #ifndef HAL_BUILD_AP_PERIPH
// note that analog will always come back as present if the pin is valid // note that analog will always come back as present if the pin is valid
if (AP_RangeFinder_analog::detect(params[instance])) { if (AP_RangeFinder_analog::detect(params[instance])) {
drivers[instance] = new AP_RangeFinder_analog(state[instance], params[instance]); _add_backend(new AP_RangeFinder_analog(state[instance], params[instance]), instance);
} }
#endif #endif
break; break;
@ -467,55 +481,55 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
#ifndef HAL_BUILD_AP_PERIPH #ifndef HAL_BUILD_AP_PERIPH
// note that this will always come back as present if the pin is valid // note that this will always come back as present if the pin is valid
if (AP_RangeFinder_HC_SR04::detect(params[instance])) { if (AP_RangeFinder_HC_SR04::detect(params[instance])) {
drivers[instance] = new AP_RangeFinder_HC_SR04(state[instance], params[instance]); _add_backend(new AP_RangeFinder_HC_SR04(state[instance], params[instance]), instance);
} }
#endif #endif
break; break;
case Type::NMEA: case Type::NMEA:
if (AP_RangeFinder_NMEA::detect(serial_instance)) { if (AP_RangeFinder_NMEA::detect(serial_instance)) {
drivers[instance] = new AP_RangeFinder_NMEA(state[instance], params[instance], serial_instance++); _add_backend(new AP_RangeFinder_NMEA(state[instance], params[instance], serial_instance++), instance);
} }
break; break;
case Type::WASP: case Type::WASP:
if (AP_RangeFinder_Wasp::detect(serial_instance)) { if (AP_RangeFinder_Wasp::detect(serial_instance)) {
drivers[instance] = new AP_RangeFinder_Wasp(state[instance], params[instance], serial_instance++); _add_backend(new AP_RangeFinder_Wasp(state[instance], params[instance], serial_instance++), instance);
} }
break; break;
case Type::BenewakeTF02: case Type::BenewakeTF02:
if (AP_RangeFinder_Benewake_TF02::detect(serial_instance)) { if (AP_RangeFinder_Benewake_TF02::detect(serial_instance)) {
drivers[instance] = new AP_RangeFinder_Benewake_TF02(state[instance], params[instance], serial_instance++); _add_backend(new AP_RangeFinder_Benewake_TF02(state[instance], params[instance], serial_instance++), instance);
} }
break; break;
case Type::BenewakeTFmini: case Type::BenewakeTFmini:
if (AP_RangeFinder_Benewake_TFMini::detect(serial_instance)) { if (AP_RangeFinder_Benewake_TFMini::detect(serial_instance)) {
drivers[instance] = new AP_RangeFinder_Benewake_TFMini(state[instance], params[instance], serial_instance++); _add_backend(new AP_RangeFinder_Benewake_TFMini(state[instance], params[instance], serial_instance++), instance);
} }
break; break;
case Type::BenewakeTF03: case Type::BenewakeTF03:
if (AP_RangeFinder_Benewake_TF03::detect(serial_instance)) { if (AP_RangeFinder_Benewake_TF03::detect(serial_instance)) {
drivers[instance] = new AP_RangeFinder_Benewake_TF03(state[instance], params[instance], serial_instance++); _add_backend(new AP_RangeFinder_Benewake_TF03(state[instance], params[instance], serial_instance++), instance);
} }
break; break;
case Type::PWM: case Type::PWM:
#ifndef HAL_BUILD_AP_PERIPH #ifndef HAL_BUILD_AP_PERIPH
if (AP_RangeFinder_PWM::detect()) { if (AP_RangeFinder_PWM::detect()) {
drivers[instance] = new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height); _add_backend(new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height), instance);
} }
#endif #endif
break; break;
case Type::BLPing: case Type::BLPing:
if (AP_RangeFinder_BLPing::detect(serial_instance)) { if (AP_RangeFinder_BLPing::detect(serial_instance)) {
drivers[instance] = new AP_RangeFinder_BLPing(state[instance], params[instance], serial_instance++); _add_backend(new AP_RangeFinder_BLPing(state[instance], params[instance], serial_instance++), instance);
} }
break; break;
case Type::Lanbao: case Type::Lanbao:
if (AP_RangeFinder_Lanbao::detect(serial_instance)) { if (AP_RangeFinder_Lanbao::detect(serial_instance)) {
drivers[instance] = new AP_RangeFinder_Lanbao(state[instance], params[instance], serial_instance++); _add_backend(new AP_RangeFinder_Lanbao(state[instance], params[instance], serial_instance++), instance);
} }
break; break;
case Type::LeddarVu8_Serial: case Type::LeddarVu8_Serial:
if (AP_RangeFinder_LeddarVu8::detect(serial_instance)) { if (AP_RangeFinder_LeddarVu8::detect(serial_instance)) {
drivers[instance] = new AP_RangeFinder_LeddarVu8(state[instance], params[instance], serial_instance++); _add_backend(new AP_RangeFinder_LeddarVu8(state[instance], params[instance], serial_instance++), instance);
} }
break; break;
@ -532,20 +546,20 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
case Type::GYUS42v2: case Type::GYUS42v2:
if (AP_RangeFinder_GYUS42v2::detect(serial_instance)) { if (AP_RangeFinder_GYUS42v2::detect(serial_instance)) {
drivers[instance] = new AP_RangeFinder_GYUS42v2(state[instance], params[instance], serial_instance++); _add_backend(new AP_RangeFinder_GYUS42v2(state[instance], params[instance], serial_instance++), instance);
} }
break; break;
case Type::SITL: case Type::SITL:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
drivers[instance] = new AP_RangeFinder_SITL(state[instance], params[instance], instance); _add_backend(new AP_RangeFinder_SITL(state[instance], params[instance], instance), instance);
#endif #endif
break; break;
case Type::MSP: case Type::MSP:
#if HAL_MSP_RANGEFINDER_ENABLED #if HAL_MSP_RANGEFINDER_ENABLED
if (AP_RangeFinder_MSP::detect()) { if (AP_RangeFinder_MSP::detect()) {
drivers[instance] = new AP_RangeFinder_MSP(state[instance], params[instance]); _add_backend(new AP_RangeFinder_MSP(state[instance], params[instance]), instance);
} }
#endif // HAL_MSP_RANGEFINDER_ENABLED #endif // HAL_MSP_RANGEFINDER_ENABLED
break; break;

9
libraries/AP_RangeFinder/AP_RangeFinder.h
View File

@ -122,7 +122,12 @@ public:
void set_log_rfnd_bit(uint32_t log_rfnd_bit) { _log_rfnd_bit = log_rfnd_bit; } void set_log_rfnd_bit(uint32_t log_rfnd_bit) { _log_rfnd_bit = log_rfnd_bit; }
// Return the number of range finder instances /*
Return the number of range finder instances. Note that if users
sets up rangefinders with a gap in the types then this is the
index of the maximum sensor ID plus one, so this gives the value
that should be used when iterating over all sensors
*/
uint8_t num_sensors(void) const { uint8_t num_sensors(void) const {
return num_instances; return num_instances;
} }
@ -204,7 +209,7 @@ private:
void detect_instance(uint8_t instance, uint8_t& serial_instance); void detect_instance(uint8_t instance, uint8_t& serial_instance);
bool _add_backend(AP_RangeFinder_Backend *driver); bool _add_backend(AP_RangeFinder_Backend *driver, uint8_t instance);
uint32_t _log_rfnd_bit = -1; uint32_t _log_rfnd_bit = -1;
void Log_RFND(); void Log_RFND();