tfmini_i2c: The number of sensors is set by parameters

src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp

@@ -44,7 +44,7 @@ using namespace time_literals;
 
 /* Configuration Constants */
 #define TFMINI_I2C_BUS_SPEED                        100000 // 100kHz bus speed.
-#define TFMINI_I2C_BASE_ADDR                        0x10   // 7-bit address is 0x70 = 112. 8-bit address is 0xE0 = 224.
+#define TFMINI_I2C_BASE_ADDR                        0x10
 
 
 #define TFMINI_I2C_MEASURE_INTERVAL                 30_ms // 60ms minimum for one sonar.
@@ -93,7 +93,8 @@ TFMINI_I2C::getTFi2cParam(uint8_t index,uint8_t type)
 int
 TFMINI_I2C::init()
 {
-	if (_p_sensor_enabled.get() == 0) {
+	uint8_t number_of_sensors = _p_sensor_enabled.get();
+	if (number_of_sensors == 0) {
 		PX4_WARN("disabled");
 		return PX4_ERROR;
 	}
@@ -104,8 +105,7 @@ TFMINI_I2C::init()
 	}
 	// Check for connected rangefinders on each i2c port by decrementing from the base address,
 	uint8_t address;
-	for (uint8_t index = 0; index < RANGE_FINDER_MAX_SENSORS ; index++) {
-		// address = param_total_addr[index];
+	for (uint8_t index = 0; index < number_of_sensors ; index++) {
 		address = (uint8_t)getTFi2cParam(index,TF_PARAM_ADDR);
 		set_device_address(address);
 		px4_usleep(_measure_interval);
@@ -216,7 +216,6 @@ TFMINI_I2C::collect()
 		if(check_checksum(raw_data,Data_Len)){
 			distance_cm = ((uint16_t)raw_data[3] << 8) | raw_data[2];
 		}
-
 	}else{
 		err_cnt += 1;
 		return -EAGAIN;
@@ -247,14 +246,6 @@ TFMINI_I2C::collect()
 	{
 		get_a_data = 1;
 	}
-	/** for test **/
-	// static uint64_t last_us ;
-	// static double dist_arr[6]={0.01,0.01,0.01,0.01,0.01,0.01};
-	// dist_arr[_sensor_index] = (double)distance_m;
-        // if(hrt_absolute_time() - last_us > 100 * 1_ms){
-        //     printf("dist:%2.2f,%2.2f,%2.2f,%2.2f,%2.2f,%2.2f\n",dist_arr[0],dist_arr[1],dist_arr[2],dist_arr[3],dist_arr[4],dist_arr[5]);
-        //     last_us = hrt_absolute_time();
-        // }
 	perf_end(_sample_perf);
 	return PX4_OK;
 }

src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.hpp

@@ -53,7 +53,6 @@
 #include <unistd.h>
 
 #include <board_config.h>
-#include <containers/Array.hpp>
 #include <drivers/device/device.h>
 #include <drivers/device/i2c.h>
 #include <drivers/drv_hrt.h>
@@ -66,9 +65,6 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/distance_sensor.h>
 
-#include <systemlib/mavlink_log.h>
-
-
 
 class TFMINI_I2C : public device::I2C, public ModuleParams, public I2CSPIDriver<TFMINI_I2C>
 {
@@ -134,8 +130,6 @@ private:
 	};
 
 	static constexpr uint8_t RANGE_FINDER_MAX_SENSORS = 6;
-	// px4::Array<uint8_t, RANGE_FINDER_MAX_SENSORS> _sensor_addresses {};
-	// px4::Array<uint8_t, RANGE_FINDER_MAX_SENSORS> _sensor_rotations {};
 
 	int _measure_interval{0};	// Initialize the measure interval for a single sensor.
 
@@ -161,7 +155,6 @@ private:
 		(ParamInt<px4::params::SENS_EN_TFI2C>)   _p_sensor_enabled
 
 	);
-    orb_advert_t		_mavlink_log_pub{nullptr};	///< mavlink log pub
     float avoid_distance;
     int entra_avoid_area{0};
     uint8_t get_a_data;