修改地址调整

src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp

@@ -69,18 +69,13 @@
 using namespace time_literals;
 
 /* 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_BASE_ADDR                        0x10
+#define TFMINI_I2C_MAX_ADDR                         0x1A
 #define TFMINI_I2C_BUS_SPEED                        100000 // 100kHz bus speed.
 
-/* TF_I2Cxx Registers addresses */
-#define TFMINI_I2C_TAKE_RANGE_REG                   0x51 // Measure range Register.
-#define TFMINI_I2C_SET_ADDRESS_1                    0xAA // Change address 1 Register.
-#define TFMINI_I2C_SET_ADDRESS_2                    0xA5 // Change address 2 Register.
-
 /* 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!).
@@ -211,7 +206,7 @@ TFMINI_I2C::collect()
     static uint64_t last_us ;
 	perf_begin(_sample_perf);
 
-    uint8_t CMD_READ_MEASUREMENT[] = { 0x5A, 0x05, 0x00, 0x07, 0x66 };
+    uint8_t CMD_READ_MEASUREMENT[] = {0x5A,0x05,0x00,0x01,0x60};
     static  uint8_t raw_data[11];
 
 	// Increment i2c adress to next sensor.
@@ -227,10 +222,16 @@ TFMINI_I2C::collect()
 
 	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;
+	}
     /////////////////////
 
     /**
@@ -269,6 +270,10 @@ TFMINI_I2C::collect()
 ///////////////////////
 	uint16_t distance_cm = distance;
 	float distance_m = static_cast<float>(distance_cm) * 1e-2f;
+	if (distance_m > 13.0f)
+	{
+		distance_m = 13.0f;
+	}
 
 	distance_sensor_s report;
 	report.current_distance = distance_m;
@@ -287,12 +292,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) {
-		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;
+	// }
 
 	perf_end(_sample_perf);
 	return PX4_OK;
@@ -359,8 +364,8 @@ TFMINI_I2C::init()
 	}
 
 	// 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--) {
+	// (TFMINI_I2C_BASE_ADDR = 0x10, TFMINI_I2C_MAX_ADDR = 0x1a).
+	for (uint8_t address = TFMINI_I2C_BASE_ADDR; address < TFMINI_I2C_MAX_ADDR ; address++) {
 		set_device_address(address);
 
 		if (measure() == PX4_OK) {
@@ -376,6 +381,8 @@ TFMINI_I2C::init()
 			}
 
 			px4_usleep(_measure_interval);
+		}else{
+			PX4_INFO("\n tf err sensor %i at address 0x%02X added\n", _sensor_count, address);
 		}
 	}
 
@@ -466,11 +473,19 @@ TFMINI_I2C::set_address(const uint8_t address)
 
 	uint8_t shifted_address = address;
 	uint8_t cmd[5] = {0x5A, 0x05, 0x0B, shifted_address,0};
-    cmd[4] = 0x5A + 0x05 + 0x0B + shifted_address;  // 最后一位和校验
+	uint8_t ck_sum =  0x5A + 0x05 + 0x0B + shifted_address;  // 最后一位和校验
+    	cmd[4] = ck_sum;
 	if (transfer(cmd, sizeof(cmd), nullptr, 0) != PX4_OK) {
 		PX4_INFO("could not set the address");
 	}
 
+            printf("{%d}set_address arr\n",__LINE__);
+            for (uint8_t i = 0; i < 5; i++)
+            {
+                printf("%02x ",cmd[i]);
+            }
+            printf("\n");
+
 	set_device_address(address);
 	PX4_INFO("device address: %u", get_device_address());
 	return PX4_OK;

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -377,13 +377,5 @@ bool FlightTaskManualAltitude::update()
 	_updateSetpoints();
 	_constraints.want_takeoff = _checkTakeoff();
 
-	static uint64_t delay_1s = hrt_absolute_time();
-	static uint64_t delt_time;
-	if (hrt_absolute_time() - delay_1s > 1 * 1000000ULL)
-	{
-		printf("update,delt:%dms \n",(hrt_absolute_time() - delt_time)/(1 * 1000ULL) );
-        delay_1s = hrt_absolute_time();
-	}
-    	delt_time = hrt_absolute_time();
 	return ret;
 }

v5_build.sh

@@ -1,6 +1,6 @@
 date -R
 starttime=`date +'%Y-%m-%d %H:%M:%S'`
-make px4_fmu-v5_default
+make px4_fmu-v5_default upload
 endtime=`date +'%Y-%m-%d %H:%M:%S'`
 date -R
 start_seconds=$(date --date="$starttime" +%s);