增加一些仿真测试打印数据

src/lib/collision_prevention/CollisionPrevention.cpp

@@ -384,6 +384,15 @@ void
 CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos,
 		const Vector2f &curr_vel)
 {
+		bool need_prt = false;
+	static uint8_t cnt;
+	cnt+=1;
+	if (cnt > 100)
+	{
+		// need_prt = true;
+		cnt = 0;
+	}
+	//////////////////////////////
 	_updateObstacleMap();
 
 	// read parameters
@@ -401,6 +410,10 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
 	const hrt_abstime constrain_time = getTime();
 	int num_fov_bins = 0;
 
+	if (need_prt)
+	{
+		printf("[clc]setpoint_length:%.2f,contime:%d,obtime:%d,delt:%d\n",(double)setpoint_length,constrain_time , _obstacle_map_body_frame.timestamp,constrain_time - _obstacle_map_body_frame.timestamp);
+	}
 	if ((constrain_time - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) {
 		if (setpoint_length > 0.001f) {
 
@@ -416,6 +429,11 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
 			// change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps
 			_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad);
 
+			if (need_prt)
+			{
+				printf("[clc]setpoint_dir = %.2f,%.2f\n",(double)setpoint_dir(0),(double)setpoint_dir(1));
+			}
+
 			// limit speed for safe flight
 			for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { // disregard unused bins at the end of the message
 
@@ -483,6 +501,10 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
 			}
 
 			setpoint = setpoint_dir * vel_max;
+			if (need_prt)
+			{
+				printf("[clc]setpoint = %.2f,%.2f\n",(double)setpoint(0),(double)setpoint(1));
+			}
 		}
 
 	} else {
@@ -509,10 +531,21 @@ void
 CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed, const Vector2f &curr_pos,
 				    const Vector2f &curr_vel)
 {
+
+	bool need_prt = false;
+	static uint8_t cnt;
+	cnt+=1;
+	if (cnt > 100)
+	{
+		// need_prt = true;
+		cnt = 0;
+	}
+	/////////////////////////////////////
 	//calculate movement constraints based on range data
 	Vector2f new_setpoint = original_setpoint;
 	_calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);
 
+
 	//warn user if collision prevention starts to interfere
 	bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed
 				      || new_setpoint(0) > original_setpoint(0) + 0.05f * max_speed
@@ -521,6 +554,10 @@ CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max
 
 	_interfering = currently_interfering;
 
+	if (need_prt)
+	{
+		printf("setpoint:%.2f,%.2f,%d\n",(double)original_setpoint(0),(double)new_setpoint(0),_interfering);
+	}
 	// publish constraints
 	collision_constraints_s	constraints{};
 	constraints.timestamp = getTime();
@@ -535,51 +572,41 @@ 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;
-	/******************************************************************/
-
 	// _calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);
 		// read parameters
-	static float min_distance;
+	static float min_distance = 0.2f;
-	static float max_distance;
+	static float max_distance = 12.0f;
 	static float up_distance;
 	static uint64_t _data_h_timestamps;
 	static uint64_t _dist_timestamps;
 
+	bool need_prt = false;
+	static uint8_t cnt;
+	cnt+=1;
+	if (cnt > 100)
+	{
+		// need_prt = true;
+		cnt = 0;
+	}
+
 	// 获取测距距离值相关
 	for (auto &dist_sens_sub : _distance_sensor_subs) {
-        	distance_sensor_s distance_sensor;
+        distance_sensor_s distance_sensor;
-
 		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_FORWARD_FACING)) {
-
+			// (distance_sensor.orientation == distance_sensor_s::ROTATION_UPWARD_FACING)) {
 				// update message description
 				_dist_timestamps = distance_sensor.timestamp;
-				max_distance = math::max(max_distance,(distance_sensor.max_distance ));
+				max_distance = math::min(max_distance,(distance_sensor.max_distance ));
-				min_distance = math::min(min_distance,(distance_sensor.min_distance ));
+				min_distance = math::max(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);
-
 			}
 		}
-
 	}
     // 获取一些参数设置
 	const float col_prev_d = _param_cp_dist.get();
@@ -588,17 +615,21 @@ CollisionPrevention::modifySetpointH(float &original_setpoint, const float max_s
 	const float xy_p = _param_mpc_xy_p.get();
 	const float max_jerk = _param_mpc_jerk_max.get();
 	const float max_accel = _param_mpc_acc_hor.get();
-
 	const float setpoint_length = -setpoint;
 	const hrt_abstime constrain_time = getTime();
 
 
-	if ((constrain_time - _dist_timestamps) < RANGE_STREAM_TIMEOUT_US) {
+    if (need_prt)
+    {
+        printf("dist:%.2f,dt:%d,%.2f,%.2f\n",(double)up_distance,constrain_time-_dist_timestamps,(double)min_distance,(double)max_distance);
+    }
+
+	if ((constrain_time - _dist_timestamps) < RANGE_STREAM_TIMEOUT_US && up_distance > min_distance && up_distance < max_distance) {
 		if (setpoint_length > 0.001f) {
 			float vel_max = setpoint_length;
-            		// 最小停留距离，测距最小值或者参数设置停留值
+            // 最小停留距离，测距最小值或者参数设置停留值
 			const float min_dist_to_keep = math::max(min_distance / 100.0f, col_prev_d);
-            		// 删除过时的值
+            // 删除过时的值
 			const hrt_abstime data_age = constrain_time - _data_h_timestamps;//TODO: _data_timestamps需要赋值
 			if (data_age > RANGE_STREAM_TIMEOUT_US) {
 				up_distance = UINT16_MAX * 0.01f;
@@ -606,75 +637,59 @@ CollisionPrevention::modifySetpointH(float &original_setpoint, const float max_s
 			const float distance = up_distance;
 			const float max_range = max_distance + 0.005f;
 
-			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);
-			float delay_distance = curr_vel_parallel * col_prev_dly;
-			if (distance < max_range) {
-				delay_distance += curr_vel_parallel * (data_age * 1e-6f);
-			}
-			const float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance);
-			const float vel_max_posctrl = xy_p * stop_distance;
-
-			const float vel_max_smooth = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_accel, stop_distance, 0.f);
-			float vel_max_bin = math::min(vel_max_posctrl, vel_max_smooth) ;
-			// constrain the velocity
-			if (vel_max_bin >= 0) {
-				vel_max = math::min(vel_max, vel_max_bin);
-			}
-
 
-		if (need_prt)
+            if (distance > min_distance && distance < UINT16_MAX * 0.01f) {
-			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);
+                // 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);
-	}else if (distance >= UINT16_MAX * 0.01f ) {
+                float delay_distance = curr_vel_parallel * col_prev_dly;
-			if (!move_no_data || (move_no_data)) {
+                if (distance < max_range) {
-					vel_max = 0.f;
+                    delay_distance += curr_vel_parallel * (data_age * 1e-6f);
-				}
+                }
-		}
+                const float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance);
-
+                const float vel_max_posctrl = xy_p * stop_distance;
-            		setpoint = -vel_max;
+
-			if (need_prt)
+                const float vel_max_smooth = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_accel, stop_distance, 0.f);
-				printf("4:vmax:%.2f\n",(double)vel_max);
+                float vel_max_bin = math::min(vel_max_posctrl, vel_max_smooth) ;
+                // constrain the velocity
+                if (vel_max_bin >= 0) {
+                    vel_max = math::min(vel_max, vel_max_bin);
+                }
+
+            }else if (distance >= UINT16_MAX * 0.01f ) {
+                if (!move_no_data) {
+                        vel_max = 0.f;
+                }
+            }
+            setpoint = -vel_max;
 		}
 	} else {
 		//allow no movement
-		float vel_max = 0.f;
+		// float vel_max = 0.f;
-		setpoint = setpoint * vel_max;
+		// setpoint = setpoint * vel_max;
-
+		// // if distance data is stale, switch to Loiter
-		// if distance data is stale, switch to Loiter
+		// if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) {
-		if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) {
+		// 	if ((constrain_time - _dist_timestamps) > TIMEOUT_HOLD_US
-
+		// 	    && getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) {
-			if ((constrain_time - _dist_timestamps) > TIMEOUT_HOLD_US
+		// 		_publishVehicleCmdDoLoiter();
-			    && getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) {
+		// 	}
-				_publishVehicleCmdDoLoiter();
+		// 	_last_timeout_warning = getTime();
-			}
+		// }
-
-			_last_timeout_warning = getTime();
-		}
-
-		if (need_prt)
-			printf("5:setpoint:%.2f,vmax:%.2f\n", (double)setpoint,(double)vel_max);
-
-
 	}
     _data_h_timestamps = getTime();
 	/******************************************************************/
-
 	//warn user if collision prevention starts to interfere
 	bool currently_interfering = (setpoint < original_setpoint - 0.05f * max_speed
 				      || setpoint > original_setpoint + 0.05f * max_speed);
-
 	_interfering = currently_interfering;
-
-
 	original_setpoint = setpoint;
-
+	// if(_interfering){
-	if (need_prt)
+	// 	static uint8_t not_cnt;
-		printf("6:intf:%d,setpoint:%.2f\n", _interfering,(double)original_setpoint);
+	// 	not_cnt += 1;
+	// 	if(not_cnt > 10){
+	// 		not_cnt = 0;
+	// 		printf("notic:%.2f\n",(double)original_setpoint);
+	// 	}
+	// }
 }
 
 void CollisionPrevention::_publishVehicleCmdDoLoiter()

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

@@ -106,23 +106,23 @@ void FlightTaskManualAltitude::_scaleSticks()
 	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);
 	/////////////////////////////////////////////////////////////////////
-	float vel_z = vel_max_z * _sticks.getPositionExpo()(2);
 	// collision prevention
+	float vel_z ;
 	if (_collision_prevention.is_active()) {
+		vel_z = vel_max_z * _sticks.getPositionExpo()(2);
 		_collision_prevention.modifySetpointH(vel_z, vel_max_z, _position(2), _velocity(2));
+		_velocity_setpoint(2) = vel_z;
+		// static uint8_t cnt;
+		// cnt+=1;
+		// if (cnt > 100)
+		// {
+		// 	printf(" vx:%.3f,vy:%.3f,vz:%.3f,vz2:%.3f\n",(double)_velocity_setpoint(0),(double)_velocity_setpoint(1),(double)_velocity_setpoint(2),(double)vel_z);
+		// 	printf(" px:%.3f,py:%.3f,pz:%.3f\n",(double)_position(0),(double)_position(1),(double)_position(2));
+		// 	cnt = 0;
+		// }
 	}
 
 
-	static uint8_t cnt;
-	cnt+=1;
-	if (cnt > 50)
-	{
-		printf(" vf:%.3f,vb:%.3f\n",(double)_velocity_setpoint(2), (double)vel_z);
-		cnt = 0;
-	}
-
-	_velocity_setpoint(2) = vel_z;
-
 }
 
 float FlightTaskManualAltitude::_applyYawspeedFilter(float yawspeed_target)
@@ -377,6 +377,20 @@ void FlightTaskManualAltitude::_updateSetpoints()
 
 	_acceleration_setpoint.xy() = sp * tanf(math::radians(_param_mpc_man_tilt_max.get())) * CONSTANTS_ONE_G;
 
+	Vector2f setpoint = _acceleration_setpoint.xy();
+	if (_collision_prevention.is_active()) {
+		_collision_prevention.modifySetpoint(setpoint, 1.0f, _position.xy(), _velocity.xy());
+
+		// static uint8_t cnt;
+		// cnt+=1;
+		// if (cnt > 100)
+		// {
+		// 	printf(" ax:%.3f,ay:%.3f,ax2:%.3f,ay2:%.3f\n",(double)_acceleration_setpoint(0),(double)_acceleration_setpoint(1),(double)setpoint(0),(double)setpoint(1));
+		// 	// printf(" px:%.3f,py:%.3f,pz:%.3f\n",(double)_position(0),(double)_position(1),(double)_position(2));
+		// 	cnt = 0;
+		// }
+	}
+
 	_updateAltitudeLock();
 	_respectGroundSlowdown();
 }