增加tf iic地址设置

src/drivers/distance_sensor/tfmini_i2c/parameters.c

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * Benewake TFmini Rangefinder Sonar (tfmini i2c)
+ * Benewake TF Rangefinder Sonar (tf i2c)
  *
  * @reboot_required true
  *
@@ -42,7 +42,21 @@
 PARAM_DEFINE_INT32(SENS_EN_TFI2C, 0);
 
 /**
- * Benewake TFmini Sensor 0 Rotation
+ * Benewake TF Sensor 0 Address
+ *
+ * This parameter defines the address of the TFi2c sensor.
+ *
+ * @reboot_required true
+ * @min 1
+ * @max 255
+ * @group Sensors
+ *
+ */
+
+PARAM_DEFINE_INT32(SENS_TF_0_ADDR, 16);
+
+/**
+ * Benewake TF Sensor 0 Rotation
  *
  * This parameter defines the rotation of the sensor relative to the platform.
  *
@@ -69,7 +83,7 @@ PARAM_DEFINE_INT32(SENS_EN_TFI2C, 0);
 PARAM_DEFINE_INT32(SENS_TF_0_ROT, 0);
 
 /**
- * Benewake TFmini Sensor 0 Max Distance
+ * Benewake TF Sensor 0 Max Distance
  *
  * This parameter defines maximum measuring distance.
  *
@@ -82,7 +96,7 @@ PARAM_DEFINE_INT32(SENS_TF_0_ROT, 0);
 PARAM_DEFINE_INT32(SENS_TF_0_MAXD, 1200);
 
 /**
- * Benewake TFmini Sensor 0 Min Distance
+ * Benewake TF Sensor 0 Min Distance
  *
  * This parameter defines min measuring distance.
  *
@@ -95,7 +109,21 @@ PARAM_DEFINE_INT32(SENS_TF_0_MAXD, 1200);
 PARAM_DEFINE_INT32(SENS_TF_0_MIND, 10);
 
 /**
- * Benewake TFmini Sensor 1 Rotation
+ * Benewake TF Sensor 1 Address
+ *
+ * This parameter defines the address of the TFi2c sensor.
+ *
+ * @reboot_required true
+ * @min 1
+ * @max 255
+ * @group Sensors
+ *
+ */
+
+PARAM_DEFINE_INT32(SENS_TF_1_ADDR, 17);
+
+/**
+ * Benewake TF Sensor 1 Rotation
  *
  * This parameter defines the rotation of the sensor relative to the platform.
  *
@@ -122,7 +150,7 @@ PARAM_DEFINE_INT32(SENS_TF_0_MIND, 10);
 PARAM_DEFINE_INT32(SENS_TF_1_ROT, 0);
 
 /**
- * Benewake TFmini Sensor 1 Max Distance
+ * Benewake TF Sensor 1 Max Distance
  *
  * This parameter defines maximum measuring distance.
  *
@@ -135,7 +163,7 @@ PARAM_DEFINE_INT32(SENS_TF_1_ROT, 0);
 PARAM_DEFINE_INT32(SENS_TF_1_MAXD, 1200);
 
 /**
- * Benewake TFmini Sensor 1 Min Distance
+ * Benewake TF Sensor 1 Min Distance
  *
  * This parameter defines min measuring distance.
  *
@@ -148,7 +176,21 @@ PARAM_DEFINE_INT32(SENS_TF_1_MAXD, 1200);
 PARAM_DEFINE_INT32(SENS_TF_1_MIND, 10);
 
 /**
- * Benewake TFmini Sensor 2 Rotation
+ * Benewake TF Sensor 2 Address
+ *
+ * This parameter defines the address of the TFi2c sensor.
+ *
+ * @reboot_required true
+ * @min 1
+ * @max 255
+ * @group Sensors
+ *
+ */
+
+PARAM_DEFINE_INT32(SENS_TF_2_ADDR, 18);
+
+/**
+ * Benewake TF Sensor 2 Rotation
  *
  * This parameter defines the rotation of the sensor relative to the platform.
  *
@@ -175,7 +217,7 @@ PARAM_DEFINE_INT32(SENS_TF_1_MIND, 10);
 PARAM_DEFINE_INT32(SENS_TF_2_ROT, 0);
 
 /**
- * Benewake TFmini Sensor 2 Max Distance
+ * Benewake TF Sensor 2 Max Distance
  *
  * This parameter defines maximum measuring distance.
  *
@@ -188,7 +230,7 @@ PARAM_DEFINE_INT32(SENS_TF_2_ROT, 0);
 PARAM_DEFINE_INT32(SENS_TF_2_MAXD, 1200);
 
 /**
- * Benewake TFmini Sensor 2 Min Distance
+ * Benewake TF Sensor 2 Min Distance
  *
  * This parameter defines min measuring distance.
  *
@@ -201,7 +243,21 @@ PARAM_DEFINE_INT32(SENS_TF_2_MAXD, 1200);
 PARAM_DEFINE_INT32(SENS_TF_2_MIND, 10);
 
 /**
- * Benewake TFmini Sensor 3 Rotation
+ * Benewake TF Sensor 3 Address
+ *
+ * This parameter defines the address of the TFi2c sensor.
+ *
+ * @reboot_required true
+ * @min 1
+ * @max 255
+ * @group Sensors
+ *
+ */
+
+PARAM_DEFINE_INT32(SENS_TF_3_ADDR, 19);
+
+/**
+ * Benewake TF Sensor 3 Rotation
  *
  * This parameter defines the rotation of the sensor relative to the platform.
  *
@@ -228,7 +284,7 @@ PARAM_DEFINE_INT32(SENS_TF_2_MIND, 10);
 PARAM_DEFINE_INT32(SENS_TF_3_ROT, 0);
 
 /**
- * Benewake TFmini Sensor 3 Max Distance
+ * Benewake TF Sensor 3 Max Distance
  *
  * This parameter defines maximum measuring distance.
  *
@@ -241,7 +297,7 @@ PARAM_DEFINE_INT32(SENS_TF_3_ROT, 0);
 PARAM_DEFINE_INT32(SENS_TF_3_MAXD, 1200);
 
 /**
- * Benewake TFmini Sensor 3 Min Distance
+ * Benewake TF Sensor 3 Min Distance
  *
  * This parameter defines min measuring distance.
  *
@@ -254,7 +310,21 @@ PARAM_DEFINE_INT32(SENS_TF_3_MAXD, 1200);
 PARAM_DEFINE_INT32(SENS_TF_3_MIND, 10);
 
 /**
- * Benewake TFmini Sensor 4 Rotation
+ * Benewake TF Sensor 4 Address
+ *
+ * This parameter defines the address of the TFi2c sensor.
+ *
+ * @reboot_required true
+ * @min 1
+ * @max 255
+ * @group Sensors
+ *
+ */
+
+PARAM_DEFINE_INT32(SENS_TF_4_ADDR, 20);
+
+/**
+ * Benewake TF Sensor 4 Rotation
  *
  * This parameter defines the rotation of the sensor relative to the platform.
  *
@@ -281,7 +351,7 @@ PARAM_DEFINE_INT32(SENS_TF_3_MIND, 10);
 PARAM_DEFINE_INT32(SENS_TF_4_ROT, 0);
 
 /**
- * Benewake TFmini Sensor 4 Max Distance
+ * Benewake TF Sensor 4 Max Distance
  *
  * This parameter defines maximum measuring distance.
  *
@@ -294,7 +364,7 @@ PARAM_DEFINE_INT32(SENS_TF_4_ROT, 0);
 PARAM_DEFINE_INT32(SENS_TF_4_MAXD, 1200);
 
 /**
- * Benewake TFmini Sensor 4 Min Distance
+ * Benewake TF Sensor 4 Min Distance
  *
  * This parameter defines min measuring distance.
  *
@@ -307,66 +377,26 @@ PARAM_DEFINE_INT32(SENS_TF_4_MAXD, 1200);
 PARAM_DEFINE_INT32(SENS_TF_4_MIND, 10);
 
 /**
- * Benewake TFmini Sensor 5 Rotation
+ * Benewake TF Sensor 5 Address
  *
- * This parameter defines the rotation of the sensor relative to the platform.
+ * This parameter defines the address of the TFi2c sensor.
  *
  * @reboot_required true
- * @min 0
- * @max 25
- * @group Sensors
- *
- * @value 0 No rotation
- * @value 1 Yaw 45°
- * @value 2 Yaw 90°
- * @value 3 Yaw 135°
- * @value 4 Yaw 180°
- * @value 5 Yaw 225°
- * @value 6 Yaw 270°
- * @value 7 Yaw 315°
- * @value 0 ROTATION_FORWARD_FACING
- * @value 2 ROTATION_RIGHT_FACING
- * @value 4 ROTATION_BACKWARD_FACING
- * @value 6 ROTATION_LEFT_FACING
- * @value 24 ROTATION_UPWARD_FACING
- * @value 25 ROTATION_DOWNWARD_FACING
- */
-PARAM_DEFINE_INT32(SENS_TF_5_ROT, 0);
-
-/**
- * Benewake TFmini Sensor 5 Max Distance
- *
- * This parameter defines maximum measuring distance.
- *
- * @reboot_required false
- * @min 5
- * @max 20000
+ * @min 1
+ * @max 255
  * @group Sensors
  *
  */
-PARAM_DEFINE_INT32(SENS_TF_5_MAXD, 1200);
 
-/**
- * Benewake TFmini Sensor 5 Min Distance
- *
- * This parameter defines min measuring distance.
- *
- * @reboot_required false
- * @min 5
- * @max 20000
- * @group Sensors
- *
- */
-PARAM_DEFINE_INT32(SENS_TF_5_MIND, 10);
+PARAM_DEFINE_INT32(SENS_TF_5_ADDR, 21);
 
 /**
- * Benewake TFmini Sensor 6 Rotation
+ * Benewake TF Sensor 5 Rotation
  *
  * This parameter defines the rotation of the sensor relative to the platform.
  *
  * @reboot_required true
  * @min 0
- * @max 7
  * @max 25
  * @group Sensors
  *
@@ -385,10 +415,10 @@ PARAM_DEFINE_INT32(SENS_TF_5_MIND, 10);
  * @value 24 ROTATION_UPWARD_FACING
  * @value 25 ROTATION_DOWNWARD_FACING
  */
-PARAM_DEFINE_INT32(SENS_TF_6_ROT, 0);
+PARAM_DEFINE_INT32(SENS_TF_5_ROT, 0);
 
 /**
- * Benewake TFmini Sensor 6 Max Distance
+ * Benewake TF Sensor 5 Max Distance
  *
  * This parameter defines maximum measuring distance.
  *
@@ -398,10 +428,10 @@ PARAM_DEFINE_INT32(SENS_TF_6_ROT, 0);
  * @group Sensors
  *
  */
-PARAM_DEFINE_INT32(SENS_TF_6_MAXD, 1200);
+PARAM_DEFINE_INT32(SENS_TF_5_MAXD, 1200);
 
 /**
- * Benewake TFmini Sensor 6 Min Distance
+ * Benewake TF Sensor 5 Min Distance
  *
  * This parameter defines min measuring distance.
  *
@@ -411,27 +441,5 @@ PARAM_DEFINE_INT32(SENS_TF_6_MAXD, 1200);
  * @group Sensors
  *
  */
-PARAM_DEFINE_INT32(SENS_TF_6_MIND, 10);
-
-/**
- * Minimum distance the vehicle should keep to all obstacles
- *
- * Only used in Position mode. Collision avoidance is disabled by setting this parameter to a negative value
- *
- * @min -1
- * @max 15
- * @unit m
- * @group Multicopter Position Control
- */
-PARAM_DEFINE_FLOAT(SENS_TF_AVD_D, 2.0f);
+PARAM_DEFINE_INT32(SENS_TF_5_MIND, 10);
 
-/**
- * Manual-Position control sub-mode
- *
- *
- * @value 0 Simple position control
- * @value 3 Smooth position control (Jerk optimized)
- * @value 4 Acceleration based input
- * @group Multicopter Position Control
- */
-PARAM_DEFINE_INT32(SENS_TF_AVD_M, 0);

src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp

@@ -174,31 +174,32 @@ private:
 	max_min_dist dist_range[6];
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::SENS_EN_TFI2C>)   _p_sensor_enabled,
+		(ParamInt<px4::params::SENS_TF_0_ADDR>)  _p_sensor0_addr,
+		(ParamInt<px4::params::SENS_TF_1_ADDR>)  _p_sensor1_addr,
+		(ParamInt<px4::params::SENS_TF_2_ADDR>)  _p_sensor2_addr,
+		(ParamInt<px4::params::SENS_TF_3_ADDR>)  _p_sensor3_addr,
+		(ParamInt<px4::params::SENS_TF_4_ADDR>)  _p_sensor4_addr,
+		(ParamInt<px4::params::SENS_TF_5_ADDR>)  _p_sensor5_addr,
 		(ParamInt<px4::params::SENS_TF_0_ROT>)  _p_sensor0_rot,
 		(ParamInt<px4::params::SENS_TF_1_ROT>)  _p_sensor1_rot,
 		(ParamInt<px4::params::SENS_TF_2_ROT>)  _p_sensor2_rot,
 		(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_0_MAXD>)  _p_tf_0_max_dist,
 		(ParamInt<px4::params::SENS_TF_1_MAXD>)  _p_tf_1_max_dist,
 		(ParamInt<px4::params::SENS_TF_2_MAXD>)  _p_tf_2_max_dist,
 		(ParamInt<px4::params::SENS_TF_3_MAXD>)  _p_tf_3_max_dist,
 		(ParamInt<px4::params::SENS_TF_4_MAXD>)  _p_tf_4_max_dist,
 		(ParamInt<px4::params::SENS_TF_5_MAXD>)  _p_tf_5_max_dist,
-		(ParamInt<px4::params::SENS_TF_6_MAXD>)  _p_tf_6_max_dist,
 		(ParamInt<px4::params::SENS_TF_0_MIND>)  _p_tf_0_min_dist,
 		(ParamInt<px4::params::SENS_TF_1_MIND>)  _p_tf_1_min_dist,
 		(ParamInt<px4::params::SENS_TF_2_MIND>)  _p_tf_2_min_dist,
 		(ParamInt<px4::params::SENS_TF_3_MIND>)  _p_tf_3_min_dist,
 		(ParamInt<px4::params::SENS_TF_4_MIND>)  _p_tf_4_min_dist,
 		(ParamInt<px4::params::SENS_TF_5_MIND>)  _p_tf_5_min_dist,
-		(ParamInt<px4::params::SENS_TF_6_MIND>)  _p_tf_6_min_dist,
-		(ParamFloat<px4::params::SENS_TF_AVD_D>)  _p_tf_avoid_dist_m,
-		(ParamInt<px4::params::SENS_TF_AVD_M>)  _p_tf_avoid_mode,
 		(ParamFloat<px4::params::CP_DIST>)   _p_cp_dist,
-        (ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode
+        	(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode
 
 	);
     orb_advert_t		_mavlink_log_pub{nullptr};	///< mavlink log pub
@@ -240,32 +241,36 @@ TFMINI_I2C::init()
 		return PX4_ERROR;
 	}
 	printf("\n--------------------\n");
-
+	const uint8_t param_total_addr[6] = { (uint8_t)_p_sensor0_addr.get(), (uint8_t)_p_sensor1_addr.get(), (uint8_t)_p_sensor2_addr.get(), \
+						(uint8_t)_p_sensor3_addr.get(),(uint8_t)_p_sensor4_addr.get(),(uint8_t)_p_sensor5_addr.get() };
+	const uint8_t param_total_rot[6] = { (uint8_t)_p_sensor0_rot.get(), (uint8_t)_p_sensor1_rot.get(), (uint8_t)_p_sensor2_rot.get(), \
+						(uint8_t)_p_sensor3_rot.get(),(uint8_t)_p_sensor4_rot.get(),(uint8_t)_p_sensor5_rot.get() };
 	// 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++) {
+	uint8_t address;
+	for (uint8_t index = 0; index < RANGE_FINDER_MAX_SENSORS ; index++) {
+		address = param_total_addr[index];
 		set_device_address(address);
 		px4_usleep(_measure_interval);
 		if (measure() == PX4_OK) {
+			px4_usleep(_measure_interval);
 			// Store I2C address
 			_sensor_addresses[_sensor_count] = address;
-			_sensor_rotations[_sensor_count] = get_sensor_rotation(_sensor_count);
+			_sensor_rotations[_sensor_count] = param_total_rot[index];
 			_sensor_count++;
             		int set_ret = set_param();
-			PX4_INFO("sensor %i at address 0x%02X ,set:%d", _sensor_count, get_device_address(),set_ret);
-
+			printf("tf[%i]: address 0x%02X ,rototion %d, set param:%d\n", _sensor_count, get_device_address(),_sensor_rotations[_sensor_count],set_ret);
+			mavlink_log_info(&_mavlink_log_pub, "sensor %i at address 0x%02X ,rotation:%d", _sensor_count, get_device_address(),_sensor_rotations[_sensor_count]);
 			if (_sensor_count >= RANGE_FINDER_MAX_SENSORS) {
 				break;
 			}
 		}else{
 
-			printf("\nerr tf[%d]:add%02x",_sensor_count,address);
+			printf("\nerr tf[%d]:address:%02x",_sensor_count,address);
 		}
 		px4_usleep(_measure_interval);
 	}
 	printf("\n--------------------\n");
-    // float avoid_dist = _p_tf_avoid_dist_m.get();
-    // int32_t avoid_mode = _p_tf_avoid_mode.get();
 
     // if(avoid_dist > 0.5f){
 
@@ -432,7 +437,7 @@ TFMINI_I2C::collect()
 	uint16_t distance_cm = distance;
 	if (distance_cm > dist_range[_sensor_index].max || distance_cm < dist_range[_sensor_index].min )	// 超出距离范围
 	{
-		distance_cm = dist_range[_sensor_index].max + 50;
+		distance_cm = dist_range[_sensor_index].max ;
 	}
 
 	float distance_m = static_cast<float>(distance_cm) * 1e-2f;
@@ -509,7 +514,6 @@ TFMINI_I2C::get_sensor_rotation(const size_t index)
 
 	case 5: return _p_sensor5_rot.get();
 
-	case 6: return _p_sensor6_rot.get();
 
 	default: return PX4_ERROR;
 	}

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

@@ -103,7 +103,7 @@ void FlightTaskManualAltitude::_scaleSticks()
 	bool need_prt = false;
 	if (hrt_absolute_time() - delay_1s > 1 * 1000000ULL)
 	{
-		need_prt = true;
+		// need_prt = true;
 		delay_1s = hrt_absolute_time();
 	}
 

v2_fmu.sh

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