AP_RangeFinder: add and use probe_i2c_buses method

Author: peterbarker

libraries/AP_RangeFinder/AP_RangeFinder.cpp

@@ -281,6 +281,24 @@ __INITFUNC__ bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend, uin
     return true;
 }
 
+/*
+ * probe all i2c buses for a backend type:
+ */
+__INITFUNC__ void RangeFinder::probe_i2c_buses(uint8_t instance, uint8_t addr, i2c_probe_fn_t detect_fn)
+{
+    FOREACH_I2C(i) {
+        auto *device_ptr = hal.i2c_mgr->get_device_ptr(i, addr);
+        if (device_ptr == nullptr) {
+            continue;
+        }
+        if (_add_backend(detect_fn(state[instance], params[instance], *device_ptr),
+                         instance)) {
+            return;
+        }
+        delete device_ptr;
+    }
+}
+
 /*
   detect if an instance of a rangefinder is connected. 
  */
@@ -308,18 +326,7 @@ __INITFUNC__ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial
         if (params[instance].address != 0) {
             addr = params[instance].address;
         }
-        FOREACH_I2C(i) {
-            auto *device_ptr = hal.i2c_mgr->get_device_ptr(i, addr);
-            if (device_ptr == nullptr) {
-                continue;
-            }
-            if (_add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(state[instance], params[instance],
-                                                                  *device_ptr),
-                             instance)) {
-                break;
-            }
-            delete device_ptr;
-        }
+        probe_i2c_buses(instance, addr, AP_RangeFinder_MaxsonarI2CXL::detect);
         break;
     }
 #endif
@@ -340,36 +347,15 @@ __INITFUNC__ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial
                                                              instance);
             }
 #else
-            FOREACH_I2C(i) {
-                auto *device_ptr = hal.i2c_mgr->get_device_ptr(i, params[instance].address);
-                if (device_ptr == nullptr) {
-                    continue;
-                }
-                if (_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], params[instance],
-                                                                     *device_ptr),
-                                 instance)) {
-                    break;
-                }
-            }
+            probe_i2c_buses(instance, params[instance].address, AP_RangeFinder_LightWareI2C::detect);
 #endif
         }
         break;
 #endif  // AP_RANGEFINDER_LWI2C_ENABLED
 #if AP_RANGEFINDER_TRI2C_ENABLED
     case Type::TRI2C:
         if (params[instance].address) {
-            FOREACH_I2C(i) {
-                auto *device_ptr = hal.i2c_mgr->get_device_ptr(i, params[instance].address);
-                if (device_ptr == nullptr) {
-                    continue;
-                }
-                if (_add_backend(AP_RangeFinder_TeraRangerI2C::detect(state[instance], params[instance],
-                                                                      *device_ptr),
-                                 instance)) {
-                    break;
-                }
-                delete device_ptr;
-            }
+            probe_i2c_buses(instance, params[instance].address, AP_RangeFinder_TeraRangerI2C::detect);
         }
         break;
 #endif
@@ -400,18 +386,7 @@ __INITFUNC__ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial
         if (params[instance].address != 0) {
             addr = params[instance].address;
         }
-        FOREACH_I2C(i) {
-            auto *device_ptr = hal.i2c_mgr->get_device_ptr(i, addr);
-            if (device_ptr == nullptr) {
-                continue;
-            }
-            if (_add_backend(AP_RangeFinder_Benewake_TFMiniPlus::detect(state[instance], params[instance],
-                                                                        *device_ptr),
-                    instance)) {
-                break;
-            }
-            delete device_ptr;
-        }
+        probe_i2c_buses(instance, addr, AP_RangeFinder_Benewake_TFMiniPlus::detect);
         break;
     }
 #endif
@@ -421,18 +396,7 @@ __INITFUNC__ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial
         if (params[instance].address != 0) {
             addr = params[instance].address;
         }
-        FOREACH_I2C(i) {
-            auto *device_ptr = hal.i2c_mgr->get_device_ptr(i, addr);
-            if (device_ptr == nullptr) {
-                continue;
-            }
-            if (_add_backend(AP_RangeFinder_Benewake_TFS20L::detect(state[instance], params[instance],
-                                                                    *device_ptr),
-                    instance)) {
-                break;
-            }
-            delete device_ptr;
-        }
+        probe_i2c_buses(instance, addr, AP_RangeFinder_Benewake_TFS20L::detect);
         break;
     }
 #endif

libraries/AP_RangeFinder/AP_RangeFinder.h

@@ -340,6 +340,10 @@ class RangeFinder
 
     bool _add_backend(AP_RangeFinder_Backend *driver, uint8_t instance, uint8_t serial_instance=0);
 
+    // search all i2c buses for a device at addr
+    using i2c_probe_fn_t = AP_RangeFinder_Backend* (*)(RangeFinder::RangeFinder_State&, AP_RangeFinder_Params&, AP_HAL::I2CDevice&);
+    void probe_i2c_buses(uint8_t instance, uint8_t addr, i2c_probe_fn_t detect_fn);
+
     uint32_t _log_rfnd_bit = -1;
     void Log_RFND() const;
 };