增加距离范围设置

src/drivers/distance_sensor/tfmini/TFMINI.cpp

@@ -82,7 +82,7 @@ TFMINI::init()
 		// is happening sometimes. this cause EKF to believe inconsistent range readings.
 		// So we set 0.4 as valid minimum.
 		_px4_rangefinder.set_min_distance(0.4f);
-		_px4_rangefinder.set_max_distance(12.0f);
+		_px4_rangefinder.set_max_distance(50.0f);
 		_px4_rangefinder.set_fov(math::radians(1.15f));
 
 		break;

src/drivers/distance_sensor/tfmini_i2c/parameters.c

@@ -48,7 +48,7 @@ PARAM_DEFINE_INT32(SENS_EN_TFI2C, 0);
  *
  * @reboot_required true
  * @min 0
- * @max 7
+ * @max 25
  * @group Sensors
  *
  * @value 0 No rotation
@@ -59,9 +59,41 @@ PARAM_DEFINE_INT32(SENS_EN_TFI2C, 0);
  * @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_0_ROT, 0);
 
+/**
+ * Benewake TFmini Sensor 0 Max Distance
+ *
+ * This parameter defines maximum measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_0_MAXD, 1200);
+
+/**
+ * Benewake TFmini Sensor 0 Min Distance
+ *
+ * This parameter defines min measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_0_MIND, 10);
+
 /**
  * Benewake TFmini Sensor 1 Rotation
  *
@@ -69,7 +101,7 @@ PARAM_DEFINE_INT32(SENS_TF_0_ROT, 0);
  *
  * @reboot_required true
  * @min 0
- * @max 7
+ * @max 25
  * @group Sensors
  *
  * @value 0 No rotation
@@ -80,9 +112,41 @@ PARAM_DEFINE_INT32(SENS_TF_0_ROT, 0);
  * @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_1_ROT, 0);
 
+/**
+ * Benewake TFmini Sensor 1 Max Distance
+ *
+ * This parameter defines maximum measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_1_MAXD, 1200);
+
+/**
+ * Benewake TFmini Sensor 1 Min Distance
+ *
+ * This parameter defines min measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_1_MIND, 10);
+
 /**
  * Benewake TFmini Sensor 2 Rotation
  *
@@ -90,7 +154,7 @@ PARAM_DEFINE_INT32(SENS_TF_1_ROT, 0);
  *
  * @reboot_required true
  * @min 0
- * @max 7
+ * @max 25
  * @group Sensors
  *
  * @value 0 No rotation
@@ -101,9 +165,41 @@ PARAM_DEFINE_INT32(SENS_TF_1_ROT, 0);
  * @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_2_ROT, 0);
 
+/**
+ * Benewake TFmini Sensor 2 Max Distance
+ *
+ * This parameter defines maximum measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_2_MAXD, 1200);
+
+/**
+ * Benewake TFmini Sensor 2 Min Distance
+ *
+ * This parameter defines min measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_2_MIND, 10);
+
 /**
  * Benewake TFmini Sensor 3 Rotation
  *
@@ -111,7 +207,7 @@ PARAM_DEFINE_INT32(SENS_TF_2_ROT, 0);
  *
  * @reboot_required true
  * @min 0
- * @max 7
+ * @max 25
  * @group Sensors
  *
  * @value 0 No rotation
@@ -122,9 +218,41 @@ PARAM_DEFINE_INT32(SENS_TF_2_ROT, 0);
  * @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_3_ROT, 0);
 
+/**
+ * Benewake TFmini Sensor 3 Max Distance
+ *
+ * This parameter defines maximum measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_3_MAXD, 1200);
+
+/**
+ * Benewake TFmini Sensor 3 Min Distance
+ *
+ * This parameter defines min measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_3_MIND, 10);
+
 /**
  * Benewake TFmini Sensor 4 Rotation
  *
@@ -132,7 +260,7 @@ PARAM_DEFINE_INT32(SENS_TF_3_ROT, 0);
  *
  * @reboot_required true
  * @min 0
- * @max 7
+ * @max 25
  * @group Sensors
  *
  * @value 0 No rotation
@@ -143,9 +271,41 @@ PARAM_DEFINE_INT32(SENS_TF_3_ROT, 0);
  * @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_4_ROT, 0);
 
+/**
+ * Benewake TFmini Sensor 4 Max Distance
+ *
+ * This parameter defines maximum measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_4_MAXD, 1200);
+
+/**
+ * Benewake TFmini Sensor 4 Min Distance
+ *
+ * This parameter defines min measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_4_MIND, 10);
+
 /**
  * Benewake TFmini Sensor 5 Rotation
  *
@@ -153,7 +313,7 @@ PARAM_DEFINE_INT32(SENS_TF_4_ROT, 0);
  *
  * @reboot_required true
  * @min 0
- * @max 7
+ * @max 25
  * @group Sensors
  *
  * @value 0 No rotation
@@ -164,9 +324,41 @@ PARAM_DEFINE_INT32(SENS_TF_4_ROT, 0);
  * @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
+ * @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);
+
 /**
  * Benewake TFmini Sensor 6 Rotation
  *
@@ -175,6 +367,7 @@ PARAM_DEFINE_INT32(SENS_TF_5_ROT, 0);
  * @reboot_required true
  * @min 0
  * @max 7
+ * @max 25
  * @group Sensors
  *
  * @value 0 No rotation
@@ -185,9 +378,41 @@ PARAM_DEFINE_INT32(SENS_TF_5_ROT, 0);
  * @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_6_ROT, 0);
 
+/**
+ * Benewake TFmini Sensor 6 Max Distance
+ *
+ * This parameter defines maximum measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_6_MAXD, 1200);
+
+/**
+ * Benewake TFmini Sensor 6 Min Distance
+ *
+ * This parameter defines min measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_6_MIND, 10);
+
 /**
  * Minimum distance the vehicle should keep to all obstacles
  *

src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp

@@ -85,8 +85,8 @@ using namespace time_literals;
 #define TFMINI_I2C_MIN_DISTANCE                     (0.10f)
 #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!).
+#define TFMINI_I2C_MEASURE_INTERVAL                 20_ms // 60ms minimum for one sonar.
+#define TFMINI_I2C_INTERVAL_BETWEEN_SUCCESIVE_FIRES 10_ms // 30ms minimum between each sonar measurement (watch out for interference!).
 
 class TFMINI_I2C : public device::I2C, public ModuleParams, public I2CSPIDriver<TFMINI_I2C>
 {
@@ -150,7 +150,7 @@ private:
 
     int set_param();
 
-	static constexpr uint8_t RANGE_FINDER_MAX_SENSORS = 4;
+	static constexpr uint8_t RANGE_FINDER_MAX_SENSORS = 6;
 	px4::Array<uint8_t, RANGE_FINDER_MAX_SENSORS> _sensor_addresses {};
 	px4::Array<uint8_t, RANGE_FINDER_MAX_SENSORS> _sensor_rotations {};
 
@@ -166,7 +166,12 @@ private:
 	perf_counter_t _sample_perf{perf_alloc(PC_COUNT, "tfmini_i2c_sample_perf")};
 
     	uint8_t crc_clc;
-
+	struct max_min_dist
+	{
+		int32_t max;
+		int32_t min;
+	};
+	max_min_dist dist_range[6];
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::SENS_EN_TFI2C>)   _p_sensor_enabled,
 		(ParamInt<px4::params::SENS_TF_0_ROT>)  _p_sensor0_rot,
@@ -176,6 +181,20 @@ private:
 		(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,
@@ -257,6 +276,19 @@ TFMINI_I2C::init()
         printf("get avoid distance:%.2fm,mode:%d\n",(double)avoid_distance,(uint8_t)_param_mpc_pos_mode.get());
     // }
 
+	dist_range[0].max = _p_tf_0_max_dist.get();
+    dist_range[1].max = _p_tf_1_max_dist.get();
+    dist_range[2].max = _p_tf_2_max_dist.get();
+    dist_range[3].max = _p_tf_3_max_dist.get();
+    dist_range[4].max = _p_tf_4_max_dist.get();
+    dist_range[5].max = _p_tf_5_max_dist.get();
+    dist_range[0].min = _p_tf_0_min_dist.get();
+    dist_range[1].min = _p_tf_1_min_dist.get();
+    dist_range[2].min = _p_tf_2_min_dist.get();
+    dist_range[3].min = _p_tf_3_min_dist.get();
+    dist_range[4].min = _p_tf_4_min_dist.get();
+    dist_range[5].min = _p_tf_5_min_dist.get();
+
 	// Return an error if no sensors were detected.
 	if (_sensor_count == 0) {
 		PX4_ERR("no sensors discovered");
@@ -388,13 +420,18 @@ 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;
+	}
+
 	float distance_m = static_cast<float>(distance_cm) * 1e-2f;
 
 	distance_sensor_s report;
 	report.current_distance = distance_m;
 	report.device_id        = get_device_id();
-	report.max_distance     = TFMINI_I2C_MAX_DISTANCE;
-	report.min_distance     = TFMINI_I2C_MIN_DISTANCE;
+	report.max_distance     = dist_range[_sensor_index].max / 100.0f;
+	report.min_distance     = dist_range[_sensor_index].min / 100.0f;
 	report.orientation      = _sensor_rotations[_sensor_index];
 	report.signal_quality   = -1;
 	report.timestamp        = hrt_absolute_time();
@@ -413,15 +450,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);
+//     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;

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);