增加高度防撞，仿真测试ok

src/lib/collision_prevention/CollisionPrevention.cpp

@@ -531,6 +531,152 @@ CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max
 	original_setpoint = new_setpoint;
 }
 
+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 max_distance;
+	static float up_distance;
+	static uint64_t _data_h_timestamps;
+	static uint64_t _dist_timestamps;
+
+	// 获取测距距离值相关
+	for (auto &dist_sens_sub : _distance_sensor_subs) {
+        	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)) {
+
+				// update message description
+				_dist_timestamps = distance_sensor.timestamp;
+				max_distance = math::max(max_distance,(distance_sensor.max_distance ));
+				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);
+
+			}
+		}
+
+	}
+    // 获取一些参数设置
+	const float col_prev_d = _param_cp_dist.get();
+	const float col_prev_dly = _param_cp_delay.get();
+	const bool move_no_data = _param_cp_go_nodata.get();
+	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 (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;
+			}
+			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)
+			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;
+				}
+		}
+
+            		setpoint = -vel_max;
+			if (need_prt)
+				printf("4:vmax:%.2f\n",(double)vel_max);
+		}
+	} else {
+		//allow no movement
+		float vel_max = 0.f;
+		setpoint = setpoint * vel_max;
+
+		// if distance data is stale, switch to Loiter
+		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) {
+				_publishVehicleCmdDoLoiter();
+			}
+
+			_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 (need_prt)
+		printf("6:intf:%d,setpoint:%.2f\n", _interfering,(double)original_setpoint);
+}
+
 void CollisionPrevention::_publishVehicleCmdDoLoiter()
 {
 	vehicle_command_s command{};

src/lib/collision_prevention/CollisionPrevention.hpp

@@ -82,6 +82,10 @@ public:
 	void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed,
 			    const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel);
 
+
+	void modifySetpointH(float &original_setpoint, const float max_speed,
+			    const float &curr_alt, const float &curr_vel_z);
+
 protected:
 
 	obstacle_distance_s _obstacle_map_body_frame {};

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

@@ -43,7 +43,8 @@
 using namespace matrix;
 
 FlightTaskManualAltitude::FlightTaskManualAltitude() :
-	_sticks(this)
+	_sticks(this),
+	_collision_prevention(this)
 {}
 
 bool FlightTaskManualAltitude::updateInitialize()
@@ -104,6 +105,24 @@ void FlightTaskManualAltitude::_scaleSticks()
 	// 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);
+	/////////////////////////////////////////////////////////////////////
+	float vel_z = vel_max_z * _sticks.getPositionExpo()(2);
+	// collision prevention
+	if (_collision_prevention.is_active()) {
+		_collision_prevention.modifySetpointH(vel_z, vel_max_z, _position(2), _velocity(2));
+	}
+
+
+	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)

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

@@ -43,6 +43,7 @@
 #include "Sticks.hpp"
 #include <lib/ecl/AlphaFilter/AlphaFilter.hpp>
 #include <uORB/Subscription.hpp>
+#include <lib/collision_prevention/CollisionPrevention.hpp>
 
 class FlightTaskManualAltitude : public FlightTask
 {
@@ -151,4 +152,6 @@ private:
 	float _dist_to_ground_lock = NAN;
 
 	AlphaFilter<matrix::Vector2f> _man_input_filter;
+	CollisionPrevention _collision_prevention;	/**< collision avoidance setpoint amendment */
+
 };