一些参数调整，fmuv2删除一些模块减小体积

boards/px4/fmu-v2/default.cmake

@@ -24,8 +24,8 @@ px4_add_board(
 		#camera_capture
 		#camera_trigger
 		#differential_pressure # all available differential pressure drivers
-		differential_pressure/ms4525
+		# differential_pressure/ms4525
-		#distance_sensor # all available distance sensor drivers
+		distance_sensor # all available distance sensor drivers
 		#distance_sensor/ll40ls
 		#distance_sensor/lightware_laser_serial
 		#dshot
@@ -60,7 +60,7 @@ px4_add_board(
 		tone_alarm
 		#uavcan
 	MODULES
-		airspeed_selector
+		# airspeed_selector
 		#attitude_estimator_q
 		battery_status
 		#camera_feedback
@@ -70,8 +70,8 @@ px4_add_board(
 		#esc_battery
 		#events
 		flight_mode_manager
-		fw_att_control
+		# fw_att_control
-		fw_pos_control_l1
+		# fw_pos_control_l1
 		#gyro_calibration
 		#gyro_fft
 		land_detector

src/drivers/distance_sensor/tfmini_i2c/parameters.c

@@ -189,106 +189,24 @@ PARAM_DEFINE_INT32(SENS_TF_5_ROT, 0);
 PARAM_DEFINE_INT32(SENS_TF_6_ROT, 0);
 
 /**
- * Benewake TFmini Sensor 7 Rotation
+ * Minimum distance the vehicle should keep to all obstacles
  *
- * This parameter defines the rotation of the sensor relative to the platform.
+ * Only used in Position mode. Collision avoidance is disabled by setting this parameter to a negative value
- *
- * @reboot_required true
- * @min 0
- * @max 7
- * @group Sensors
  *
- * @value 0 No rotation
+ * @min -1
- * @value 1 Yaw 45°
+ * @max 15
- * @value 2 Yaw 90°
+ * @unit m
- * @value 3 Yaw 135°
+ * @group Multicopter Position Control
- * @value 4 Yaw 180°
- * @value 5 Yaw 225°
- * @value 6 Yaw 270°
- * @value 7 Yaw 315°
  */
-PARAM_DEFINE_INT32(SENS_TF_7_ROT, 0);
+PARAM_DEFINE_FLOAT(SENS_TF_AVD_D, 2.0f);
 
 /**
- * Benewake TFmini Sensor 8 Rotation
+ * Manual-Position control sub-mode
  *
- * This parameter defines the rotation of the sensor relative to the platform.
  *
- * @reboot_required true
+ * @value 0 Simple position control
- * @min 0
+ * @value 3 Smooth position control (Jerk optimized)
- * @max 7
+ * @value 4 Acceleration based input
- * @group Sensors
+ * @group Multicopter Position Control
- *
- * @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°
- */
-PARAM_DEFINE_INT32(SENS_TF_8_ROT, 0);
-
-/**
- * Benewake TFmini Sensor 9 Rotation
- *
- * This parameter defines the rotation of the sensor relative to the platform.
- *
- * @reboot_required true
- * @min 0
- * @max 7
- * @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°
- */
-PARAM_DEFINE_INT32(SENS_TF_9_ROT, 0);
-
-/**
- * Benewake TFmini Sensor 10 Rotation
- *
- * This parameter defines the rotation of the sensor relative to the platform.
- *
- * @reboot_required true
- * @min 0
- * @max 7
- * @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°
- */
-PARAM_DEFINE_INT32(SENS_TF_10_ROT, 0);
-
-/**
- * Benewake TFmini Sensor 12 Rotation
- *
- * This parameter defines the rotation of the sensor relative to the platform.
- *
- * @reboot_required true
- * @min 0
- * @max 7
- * @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°
  */
-PARAM_DEFINE_INT32(SENS_TF_11_ROT, 0);
+PARAM_DEFINE_INT32(SENS_TF_AVD_M, 0);

src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp

@@ -66,6 +66,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/distance_sensor.h>
 
+#include <systemlib/mavlink_log.h>
 using namespace time_literals;
 
 #define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0]))
@@ -174,8 +175,16 @@ 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,
+		(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
+
 	);
+    orb_advert_t		_mavlink_log_pub{nullptr};	///< mavlink log pub
+    float avoid_distance;
+    int entra_avoid_area{0};
 };
 
 TFMINI_I2C::TFMINI_I2C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address) :
@@ -235,6 +244,18 @@ TFMINI_I2C::init()
 		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){
+
+
+        // _p_cp_dist.set(avoid_dist);
+        // _param_mpc_pos_mode.set(avoid_mode);
+        // printf("set avoid distance:%.2fm,mode:%d",(double)avoid_dist,avoid_mode);
+        avoid_distance = _p_cp_dist.get();
+        printf("get avoid distance:%.2fm,mode:%d\n",(double)avoid_distance,(uint8_t)_param_mpc_pos_mode.get());
+    // }
 
 	// Return an error if no sensors were detected.
 	if (_sensor_count == 0) {
@@ -392,6 +413,15 @@ TFMINI_I2C::collect()
 	// 	perf_end(_sample_perf);
 	// 	return ret_val;
 	// }
+    if(avoid_distance - distance_m > 0.1f && !entra_avoid_area){
+        mavlink_log_info(&_mavlink_log_pub, "(%d)Enter obstacle avoidance range : %.2f\n",_sensor_index,(double)distance_m);
+        printf("(%d)Enter obstacle avoidance range : %.2f\n",_sensor_index,(double)distance_m);
+        entra_avoid_area = 1;
+    }else if(distance_m - avoid_distance > 0.1f && entra_avoid_area){
+        entra_avoid_area = 0;
+        printf("(%d)Out obstacle avoidance range : %.2f\n",_sensor_index,(double)distance_m);
+
+    }
 
 	perf_end(_sample_perf);
 	return PX4_OK;