遥控器测试高度防撞OK

fmuv3_build.sh

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

src/lib/collision_prevention/CollisionPrevention.cpp

@@ -38,6 +38,7 @@
  */
 
 #include "CollisionPrevention.hpp"
+#include <drivers/drv_hrt.h>
 
 using namespace matrix;
 using namespace time_literals;
@@ -535,6 +536,15 @@ void
 CollisionPrevention::modifySetpointH(float &original_setpoint, const float max_speed, const float &curr_pos,
 				    const float &curr_vel)
 {
+
+	static uint64_t delay_1s = hrt_absolute_time();
+	bool need_prt = false;
+	if (hrt_absolute_time() - delay_1s > 1 * 1000000ULL)
+	{
+		// need_prt = true;
+		delay_1s = hrt_absolute_time();
+	}
+
 	//calculate movement constraints based on range data
 	float setpoint = original_setpoint;
 	/******************************************************************/
@@ -554,6 +564,8 @@ CollisionPrevention::modifySetpointH(float &original_setpoint, const float max_s
         if (dist_sens_sub.update(&distance_sensor)) {
             // consider only instances with valid data and orientations useful for collision prevention
             // 距离值有效，并且是朝上的距离
+		// if (need_prt)
+		// 	printf("0.1:ts:%lld,rot:%d\n",distance_sensor.timestamp,distance_sensor.orientation);
             if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) &&
                 (distance_sensor.orientation == distance_sensor_s::ROTATION_UPWARD_FACING)) {
 
@@ -563,8 +575,14 @@ CollisionPrevention::modifySetpointH(float &original_setpoint, const float max_s
                 min_distance = math::min(min_distance,(distance_sensor.min_distance ));
 
                 up_distance = distance_sensor.current_distance;
+		// if (need_prt)
+		// 	printf("1:dist up:%.2f,max:%.2f,min:%.2f\n",(double)up_distance,(double)max_distance,(double)min_distance);
+
             }
         }
+
+	// if (need_prt)
+	// 	printf("1.1:update:%d,max:%.2f,min:%.2fv\n",dist_sens_sub.update(&distance_sensor),(double)max_distance,(double)min_distance);
     }
     // 获取一些参数设置
 	const float col_prev_d = _param_cp_dist.get();
@@ -574,7 +592,7 @@ CollisionPrevention::modifySetpointH(float &original_setpoint, const float max_s
 	const float max_jerk = _param_mpc_jerk_max.get();
 	const float max_accel = _param_mpc_acc_hor.get();
 
-	const float setpoint_length = setpoint;
+	const float setpoint_length = -setpoint;
 	const hrt_abstime constrain_time = getTime();
 
 
@@ -591,7 +609,9 @@ CollisionPrevention::modifySetpointH(float &original_setpoint, const float max_s
             const float distance = up_distance;
             const float max_range = max_distance + 0.005f;
 
-            if (distance > _obstacle_map_body_frame.min_distance && distance < UINT16_MAX * 0.01f) {
+		if (need_prt)
+			printf("2:age:%lld,dist:%.2f,max:%.2f\n",data_age,(double)distance,(double)max_range);
+            if (distance > min_distance && distance < UINT16_MAX * 0.01f) {
 
                     // calculate max allowed velocity with a P-controller (same gain as in the position controller)
                     const float curr_vel_parallel = math::max(0.f, curr_vel);
@@ -609,13 +629,19 @@ CollisionPrevention::modifySetpointH(float &original_setpoint, const float max_s
                         vel_max = math::min(vel_max, vel_max_bin);
                     }
 
+
+		if (need_prt)
+			printf("3:parallel:%.2f,posctrl:%.2f,stop_d:%.2f,vmax:%.2f\n",(double)curr_vel_parallel,(double)vel_max_posctrl,(double)stop_distance,(double)vel_max);
+
             }else if (distance >= UINT16_MAX * 0.01f ) {
-					if (!move_no_data || (move_no_data)) {
-						vel_max = 0.f;
-					}
+			if (!move_no_data || (move_no_data)) {
+					vel_max = 0.f;
 				}
+		}
 
-            setpoint = vel_max;
+            		setpoint = -vel_max;
+			if (need_prt)
+				printf("4:vmax:%.2f\n",(double)vel_max);
 		}
 	} else {
 		//allow no movement
@@ -633,6 +659,9 @@ CollisionPrevention::modifySetpointH(float &original_setpoint, const float max_s
 			_last_timeout_warning = getTime();
 		}
 
+		if (need_prt)
+			printf("5:setpoint:%.2f,vmax:%.2f\n", (double)setpoint,(double)vel_max);
+
 
 	}
     _data_h_timestamps = getTime();
@@ -646,6 +675,9 @@ CollisionPrevention::modifySetpointH(float &original_setpoint, const float max_s
 
 
 	original_setpoint = setpoint;
+
+	if (need_prt)
+		printf("6:intf:%d,setpoint:%.2f\n", _interfering,(double)original_setpoint);
 }
 
 void CollisionPrevention::_publishVehicleCmdDoLoiter()

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

@@ -99,30 +99,39 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
 
 void FlightTaskManualAltitude::_scaleSticks()
 {
+	static uint64_t delay_1s = hrt_absolute_time();
+	bool need_prt = false;
+	if (hrt_absolute_time() - delay_1s > 1 * 1000000ULL)
+	{
+		need_prt = true;
+		delay_1s = hrt_absolute_time();
+	}
+
 	// Use stick input with deadzone, exponential curve and first order lpf for yawspeed
 	const float yawspeed_target = _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get());
 	_yawspeed_setpoint = _applyYawspeedFilter(yawspeed_target);
 
 	// Use sticks input with deadzone and exponential curve for vertical velocity
 	const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
-	_velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2);
+	// _velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2);
 
-	float vel_z = _velocity_setpoint(2) ;
+	float vel_z = vel_max_z * _sticks.getPositionExpo()(2);
 	// collision prevention
 	if (_collision_prevention.is_active()) {
+		if (need_prt)
+			printf("\n1:vel_max_z:%.2f\n",(double)vel_z);
 		_collision_prevention.modifySetpointH(vel_z, vel_max_z, _position(2), _velocity(2));
+		if (need_prt)
+			printf("2:vel_max_z:%.2f\n",(double)vel_z);
 	}
 
-	static uint64_t delay_1s = hrt_absolute_time();
-	static uint64_t delt_time;
-	if (hrt_absolute_time() - delay_1s > 1 * 1000000ULL)
+	_velocity_setpoint(2) = vel_z;
+	if (need_prt)
 	{
-		printf("update,delt:%dms ,vf:%.3f,vb:%.3f\n",(hrt_absolute_time() - delt_time)/(1 * 1000ULL),(double)_velocity_setpoint(2), (double)vel_z);
-		printf("vel_max_z:%.2f,alt:%.2f,vz:%.2fv\n",(double)vel_max_z,(double)_position(2),(double)_velocity(2));
-        delay_1s = hrt_absolute_time();
+		printf(" vf:%.3f,vb:%.3f\n",(double)_velocity_setpoint(2), (double)vel_z);
+		// printf("vel_max_z:%.2f,alt:%.2f,vz:%.2fv\n",(double)vel_max_z,(double)_position(2),(double)_velocity(2));
+		printf("active:%d,vel_max_z:%.2f,alt:%.2f,vz:%.2fv\n",_collision_prevention.is_active(),(double)vel_max_z,(double)_position(2),(double)_velocity(2));
 	}
-    	delt_time = hrt_absolute_time();
-	_velocity_setpoint(2) = vel_z;
 
 }