TFMINI 调整

ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s

@@ -71,6 +71,7 @@ param set-default SENS_BOARD_ROT 10
 param set-default SENS_FLOW_ROT 4
 # TFMini on TELEM3
 param set-default SENS_TFMINI_CFG 103
+param set-default SENS_TF_SER_ROT 25
 
 # Smart Battery
 param set-default SENS_EN_BATT 1

src/drivers/distance_sensor/tfmini/TFMINI.cpp

@@ -73,6 +73,13 @@ int
 TFMINI::init()
 {
 	int32_t hw_model = 1; // only one model so far...
+	param_t _param_tf_rot = param_find("SENS_TF_S1_ROT");
+	param_t _param_dist_max = param_find("SENS_TF_S_MAXD");
+	param_t _param_dist_min = param_find("SENS_TF_S_MIND");
+	int32_t tf_rot,dist_max,dist_min;
+	param_get(_param_tf_rot, &tf_rot);
+	param_get(_param_dist_max, &dist_max);
+	param_get(_param_dist_min, &dist_min);
 
 	switch (hw_model) {
 	case 1: // TFMINI (12m, 100 Hz)
@@ -81,8 +88,9 @@ TFMINI::init()
 		// 0.3 is too close to minimum so chattering of invalid sensor decision
 		// 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(50.0f);
+		_px4_rangefinder.set_min_distance(dist_min/100.0f);
+		_px4_rangefinder.set_max_distance(dist_max/100.0f);
+		_px4_rangefinder.set_orientation(tf_rot);
 		_px4_rangefinder.set_fov(math::radians(1.15f));
 
 		break;

src/drivers/distance_sensor/tfmini/TFMINI.hpp

@@ -54,6 +54,7 @@
 #include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
 #include <uORB/topics/distance_sensor.h>
 
+#include <px4_platform_common/module_params.h>
 #include "tfmini_parser.h"
 
 #define TFMINI_DEFAULT_PORT	"/dev/ttyS3"
@@ -97,4 +98,6 @@ private:
 	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")};
 	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
 
+protected:
+
 };

src/drivers/distance_sensor/tfmini/module.yaml

@@ -1,7 +1,27 @@
 module_name: Benewake TFmini Rangefinder
 serial_config:
-    - command: tfmini start -d ${SERIAL_DEV}
+    - command: tfmini start -d ${SERIAL_DEV} -R SENS_TF_SER_ROT
       port_config_param:
         name: SENS_TFMINI_CFG
         group: Sensors
 
+
+parameters:
+    - group: Sensors
+      definitions:
+        SENS_TF_S1_ROT:
+            description:
+                short: Distance Sensor Rotation
+                long: |
+                    Distance Sensor Rotation as MAV_SENSOR_ORIENTATION enum
+
+            type: enum
+            values:
+                25: ROTATION_DOWNWARD_FACING
+                24: ROTATION_UPWARD_FACING
+                12: ROTATION_BACKWARD_FACING
+                0: ROTATION_FORWARD_FACING
+                6: ROTATION_LEFT_FACING
+                2: ROTATION_RIGHT_FACING
+            reboot_required: true
+            default: 25

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

@@ -377,13 +377,13 @@ 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();
+	// 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;
 }