tf iic 一些修改

src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp

@@ -204,6 +204,7 @@ private:
     orb_advert_t		_mavlink_log_pub{nullptr};	///< mavlink log pub
     float avoid_distance;
     int entra_avoid_area{0};
+    uint8_t get_a_data;
 };
 
 TFMINI_I2C::TFMINI_I2C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address) :
@@ -289,6 +290,7 @@ TFMINI_I2C::init()
     dist_range[4].min = _p_tf_4_min_dist.get();
     dist_range[5].min = _p_tf_5_min_dist.get();
 
+	get_a_data = 0;
 	// Return an error if no sensors were detected.
 	if (_sensor_count == 0) {
 		PX4_ERR("no sensors discovered");
@@ -357,7 +359,6 @@ TFMINI_I2C::collect()
     static uint64_t last_us ;
 	perf_begin(_sample_perf);
 
-    uint8_t CMD_READ_MEASUREMENT[] = { 0x5A, 0x05, 0x00, 0x07, 0x66 };
     static  uint8_t raw_data[Data_Len];
 
 	// Increment i2c adress to next sensor.
@@ -369,18 +370,27 @@ 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, Data_Len);
+	int ret_val;
+	if (1)
+	{
+    		uint8_t CMD_READ_MEASUREMENT[] = { 0x5A, 0x05, 0x00, 0x07, 0x66 };
+		ret_val = tfi2c_transfer(CMD_READ_MEASUREMENT, 5, raw_data, Data_Len);
+	}else
+	{
+		ret_val = tfi2c_transfer(nullptr, 0, 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;
-        }
+        // 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;
 	}
     /////////////////////
@@ -442,6 +452,10 @@ TFMINI_I2C::collect()
 
 	int instance_id;
 	orb_publish_auto(ORB_ID(distance_sensor), &_distance_sensor_topic, &report, &instance_id);
+	if (!get_a_data)
+	{
+		get_a_data = 1;
+	}
 
 	// Begin the next measurement.
 	// if (measure() != PX4_OK) {