tfmini 增加初始配置,地址增加到0x16

src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp

@@ -68,9 +68,11 @@
 
 using namespace time_literals;
 
+#define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0]))
+
 /* Configuration Constants */
 #define TFMINI_I2C_BASE_ADDR                        0x10   // 7-bit address is 0x70 = 112. 8-bit address is 0xE0 = 224.
-#define TFMINI_I2C_MIN_ADDR                         0x0f   // 7-bit address is 0x5A = 90.  8-bit address is 0xB4 = 180.
+#define TFMINI_I2C_MAX_ADDR                         0x16   // 7-bit address is 0x5A = 90.  8-bit address is 0xB4 = 180.
 #define TFMINI_I2C_BUS_SPEED                        100000 // 100kHz bus speed.
 
 /* TF_I2Cxx Registers addresses */
@@ -80,7 +82,7 @@ using namespace time_literals;
 
 /* Device limits */
 #define TFMINI_I2C_MIN_DISTANCE                     (0.10f)
-#define TFMINI_I2C_MAX_DISTANCE                     (8.00f)
+#define TFMINI_I2C_MAX_DISTANCE                     (12.00f)
 
 #define TFMINI_I2C_MEASURE_INTERVAL                 100_ms // 60ms minimum for one sonar.
 #define TFMINI_I2C_INTERVAL_BETWEEN_SUCCESIVE_FIRES 100_ms // 30ms minimum between each sonar measurement (watch out for interference!).
@@ -145,6 +147,8 @@ private:
     int tfi2c_transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len);
     int check_checksum(uint8_t *arr, int pkt_len);
 
+    int set_param();
+
 	static constexpr uint8_t RANGE_FINDER_MAX_SENSORS = 4;
 	px4::Array<uint8_t, RANGE_FINDER_MAX_SENSORS> _sensor_addresses {};
 	px4::Array<uint8_t, RANGE_FINDER_MAX_SENSORS> _sensor_rotations {};
@@ -170,12 +174,7 @@ private:
 		(ParamInt<px4::params::SENS_TF_3_ROT>)  _p_sensor3_rot,
 		(ParamInt<px4::params::SENS_TF_4_ROT>)  _p_sensor4_rot,
 		(ParamInt<px4::params::SENS_TF_5_ROT>)  _p_sensor5_rot,
-		(ParamInt<px4::params::SENS_TF_6_ROT>)  _p_sensor6_rot,
+		(ParamInt<px4::params::SENS_TF_6_ROT>)  _p_sensor6_rot
-		(ParamInt<px4::params::SENS_TF_7_ROT>)  _p_sensor7_rot,
-		(ParamInt<px4::params::SENS_TF_8_ROT>)  _p_sensor8_rot,
-		(ParamInt<px4::params::SENS_TF_9_ROT>)  _p_sensor9_rot,
-		(ParamInt<px4::params::SENS_TF_10_ROT>) _p_sensor10_rot,
-		(ParamInt<px4::params::SENS_TF_11_ROT>) _p_sensor11_rot
 	);
 };
 
@@ -199,6 +198,100 @@ TFMINI_I2C::~TFMINI_I2C()
 	perf_free(_sample_perf);
 }
 
+int
+TFMINI_I2C::init()
+{
+	if (_p_sensor_enabled.get() == 0) {
+		PX4_WARN("disabled");
+		return PX4_ERROR;
+	}
+
+	// Initialize the I2C device
+	if (I2C::init() != OK) {
+		return PX4_ERROR;
+	}
+	printf("\n--------------------\n");
+
+	// Check for connected rangefinders on each i2c port by decrementing from the base address,
+	// (TFMINI_I2C_BASE_ADDR = 112, TFMINI_I2C_MAX_ADDR = 90).
+	for (uint8_t address = TFMINI_I2C_BASE_ADDR; address < TFMINI_I2C_MAX_ADDR ; address++) {
+		set_device_address(address);
+		px4_usleep(_measure_interval);
+		if (measure() == PX4_OK) {
+			// Store I2C address
+			_sensor_addresses[_sensor_count] = address;
+			_sensor_rotations[_sensor_count] = get_sensor_rotation(_sensor_count);
+			_sensor_count++;
+            int set_ret = set_param();
+			PX4_INFO("sensor %i at address 0x%02X ,set:%d", _sensor_count, get_device_address(),set_ret);
+
+			if (_sensor_count >= RANGE_FINDER_MAX_SENSORS) {
+				break;
+			}
+		}else{
+
+			printf("\nerr tf[%d]:add%02x",_sensor_count,address);
+		}
+		px4_usleep(_measure_interval);
+	}
+	printf("\n--------------------\n");
+
+	// Return an error if no sensors were detected.
+	if (_sensor_count == 0) {
+		PX4_ERR("no sensors discovered");
+		return PX4_ERROR;
+	}
+
+	// If more than one sonar is detected, adjust the meaure interval to avoid sensor interference.
+	if (_sensor_count > 1) {
+		_measure_interval = TFMINI_I2C_INTERVAL_BETWEEN_SUCCESIVE_FIRES;
+	}
+
+	PX4_INFO("Total sensors connected: %i", _sensor_count);
+	return PX4_OK;
+}
+int
+TFMINI_I2C::set_param()
+{
+    int ret;
+
+    const uint8_t CMD_SYSTEM_RESET[] =       { 0x5A, 0x04, 0x04, 0x62 };
+    const uint8_t CMD_OUTPUT_FORMAT_CM[] =   { 0x5A, 0x05, 0x05, 0x01, 0x65 };
+    const uint8_t CMD_ENABLE_DATA_OUTPUT[] = { 0x5A, 0x05, 0x07, 0x01, 0x67 };
+    const uint8_t CMD_FRAME_RATE_100HZ[] =   { 0x5A, 0x06, 0x03, 0x64, 0x00, 0xC7 };
+    const uint8_t CMD_SAVE_SETTINGS[] =      { 0x5A, 0x04, 0x11, 0x6F };
+    const uint8_t *cmds[] = {
+        CMD_OUTPUT_FORMAT_CM,
+        CMD_FRAME_RATE_100HZ,
+        CMD_ENABLE_DATA_OUTPUT,
+        CMD_SAVE_SETTINGS,
+    };
+    for (uint8_t i = 0; i < ARRAY_SIZE(cmds); i++) {
+            ret = tfi2c_transfer(cmds[i], cmds[i][1], nullptr, 0);
+            if (!ret) {
+                PX4_INFO(": Unable to set configuration register %u\n",cmds[i][2]);
+               return ret;
+            }
+            px4_usleep(100_ms);
+    }
+		tfi2c_transfer(CMD_SYSTEM_RESET, sizeof(CMD_SYSTEM_RESET), nullptr, 0);
+
+    return PX4_OK;
+}
+int
+TFMINI_I2C::measure()
+{
+	// Send the command to take a measurement.
+    // const uint8_t CMD_FW_VERSION[] = { 0x5A, 0x04, 0x01, 0x5F };
+    // const uint8_t CMD_FW_VERSION[] = { 0x5A, 0x04, 0x01, 0x5F };
+    const uint8_t CMD_READ_MEASUREMENT[] = {0x5A,0x05,0x00,0x01,0x60 };
+
+	// uint8_t cmd = TFMINI_I2C_TAKE_RANGE_REG;
+	int ret_val = transfer(CMD_READ_MEASUREMENT, 5, nullptr, 0);
+
+	return ret_val;
+}
+
 int
 TFMINI_I2C::collect()
 {
@@ -207,12 +300,12 @@ TFMINI_I2C::collect()
     // uint16_t strength;
     // uint32_t timestamp;
     static uint32_t err_cnt;
-
+    const uint8_t Data_Len = 11;
     static uint64_t last_us ;
 	perf_begin(_sample_perf);
 
     uint8_t CMD_READ_MEASUREMENT[] = { 0x5A, 0x05, 0x00, 0x07, 0x66 };
-    static  uint8_t raw_data[11];
+    static  uint8_t raw_data[Data_Len];
 
 	// Increment i2c adress to next sensor.
 	_sensor_index++;
@@ -223,12 +316,18 @@ TFMINI_I2C::collect()
 
 	// Transfer data from the bus.
 	// int ret_val = transfer(CMD_READ_MEASUREMENT, 5, raw_data, 11);
-	int ret_val = tfi2c_transfer(CMD_READ_MEASUREMENT, 5, raw_data, 11);
+	int ret_val = tfi2c_transfer(CMD_READ_MEASUREMENT, 5, raw_data, Data_Len);
 
 	if (ret_val < 0) {
 		PX4_ERR("sensor %i read failed, address: 0x%02X", _sensor_index, get_device_address());
 		perf_count(_comms_error);
 		perf_end(_sample_perf);
+        if (measure() != PX4_OK) {
+            PX4_INFO("sensor %i measurement error, address 0x%02X", _sensor_index, get_device_address());
+            perf_count(_comms_error);
+            perf_end(_sample_perf);
+            return ret_val;
+        }
 		return ret_val;
 	}
     /////////////////////
@@ -244,7 +343,7 @@ TFMINI_I2C::collect()
     */
     if(raw_data[0] == 0x59 && raw_data[1] == 0x59)
     {
-        if(check_checksum(raw_data,11)){
+        if(check_checksum(raw_data,Data_Len)){
             distance = ((uint16_t)raw_data[3] << 8) | raw_data[2];
             // strength = ((uint16_t)raw_data[5] << 8) | raw_data[4];
             // timestamp = ((uint32_t)raw_data[9] << 8) | ((uint32_t)raw_data[8] << 8) | ((uint32_t)raw_data[7] << 8) | raw_data[6];
@@ -254,13 +353,13 @@ TFMINI_I2C::collect()
         err_cnt += 1;
         if(hrt_absolute_time() - last_us > 2 * 1_s){
             printf("{%d}err arr\n",__LINE__);
-            for (size_t i = 0; i < 11; i++)
+            for (size_t i = 0; i < Data_Len; i++)
             {
                 printf("%02x ",raw_data[i]);
             }
             printf("\n");
 
-            printf("{%d}errcnt:%d, head %02x,%02x,crc: %02x ,clc:%02x\n",__LINE__,err_cnt,raw_data[0],raw_data[1],raw_data[10],crc_clc);
+            printf("{%d}errcnt:%d, head %02x,%02x,crc: %02x ,clc:%02x\n",__LINE__,err_cnt,raw_data[0],raw_data[1],raw_data[Data_Len - 1],crc_clc);
             last_us = hrt_absolute_time();
             err_cnt = 0;
         }
@@ -287,12 +386,12 @@ TFMINI_I2C::collect()
 	orb_publish_auto(ORB_ID(distance_sensor), &_distance_sensor_topic, &report, &instance_id);
 
 	// Begin the next measurement.
-	if (measure() != PX4_OK) {
+	// if (measure() != PX4_OK) {
-		PX4_INFO("sensor %i measurement error, address 0x%02X", _sensor_index, get_device_address());
+	// 	PX4_INFO("sensor %i measurement error, address 0x%02X", _sensor_index, get_device_address());
-		perf_count(_comms_error);
+	// 	perf_count(_comms_error);
-		perf_end(_sample_perf);
+	// 	perf_end(_sample_perf);
-		return ret_val;
+	// 	return ret_val;
-	}
+	// }
 
 	perf_end(_sample_perf);
 	return PX4_OK;
@@ -331,68 +430,10 @@ TFMINI_I2C::get_sensor_rotation(const size_t index)
 
 	case 6: return _p_sensor6_rot.get();
 
-	case 7: return _p_sensor7_rot.get();
-
-	case 8: return _p_sensor8_rot.get();
-
-	case 9: return _p_sensor9_rot.get();
-
-	case 10: return _p_sensor10_rot.get();
-
-	case 11: return _p_sensor11_rot.get();
-
 	default: return PX4_ERROR;
 	}
 }
 
-int
-TFMINI_I2C::init()
-{
-	if (_p_sensor_enabled.get() == 0) {
-		PX4_WARN("disabled");
-		return PX4_ERROR;
-	}
-
-	// Initialize the I2C device
-	if (I2C::init() != OK) {
-		return PX4_ERROR;
-	}
-
-	// Check for connected rangefinders on each i2c port by decrementing from the base address,
-	// (TFMINI_I2C_BASE_ADDR = 112, TFMINI_I2C_MIN_ADDR = 90).
-	for (uint8_t address = TFMINI_I2C_BASE_ADDR; address > TFMINI_I2C_MIN_ADDR ; address--) {
-		set_device_address(address);
-
-		if (measure() == PX4_OK) {
-			// Store I2C address
-			_sensor_addresses[_sensor_count] = address;
-			_sensor_rotations[_sensor_count] = get_sensor_rotation(_sensor_count);
-			_sensor_count++;
-
-			PX4_INFO("sensor %i at address 0x%02X added", _sensor_count, get_device_address());
-
-			if (_sensor_count >= RANGE_FINDER_MAX_SENSORS) {
-				break;
-			}
-
-			px4_usleep(_measure_interval);
-		}
-	}
-
-	// Return an error if no sensors were detected.
-	if (_sensor_count == 0) {
-		PX4_ERR("no sensors discovered");
-		return PX4_ERROR;
-	}
-
-	// If more than one sonar is detected, adjust the meaure interval to avoid sensor interference.
-	if (_sensor_count > 1) {
-		_measure_interval = TFMINI_I2C_INTERVAL_BETWEEN_SUCCESIVE_FIRES;
-	}
-
-	PX4_INFO("Total sensors connected: %i", _sensor_count);
-	return PX4_OK;
-}
 
 int
 TFMINI_I2C::tfi2c_transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len)
@@ -417,19 +458,6 @@ TFMINI_I2C::tfi2c_transfer(const uint8_t *send, const unsigned send_len, uint8_t
 	return PX4_ERROR;
 }
 
-int
-TFMINI_I2C::measure()
-{
-	// Send the command to take a measurement.
-    // const uint8_t CMD_FW_VERSION[] = { 0x5A, 0x04, 0x01, 0x5F };
-    // const uint8_t CMD_FW_VERSION[] = { 0x5A, 0x04, 0x01, 0x5F };
-    const uint8_t CMD_READ_MEASUREMENT[] = { 0x5A, 0x05, 0x00, 0x07, 0x66 };
-
-	// uint8_t cmd = TFMINI_I2C_TAKE_RANGE_REG;
-	int ret_val = transfer(CMD_READ_MEASUREMENT, 5, nullptr, 0);
-
-	return ret_val;
-}
 
 void
 TFMINI_I2C::print_status()