Merge branch 'tf-1.12' of git.nebee.top:zrzk/px4_autopilot into tf-1.12

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

@@ -44,7 +44,8 @@
 using namespace matrix;
 
 FlightTaskManualAltitude::FlightTaskManualAltitude() :
-	_sticks(this)
+	_sticks(this),
+	_collision_prevention(this)
 {}
 
 bool FlightTaskManualAltitude::updateInitialize()
@@ -105,6 +106,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 = _velocity_setpoint(2) ;
+	// collision prevention
+	if (_collision_prevention.is_active()) {
+		_collision_prevention.modifySetpointH(vel_z, vel_max_z, _position(2), _velocity(2));
+	}
+
+	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 ,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();
+	}
+    	delt_time = hrt_absolute_time();
+	_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 */
+
 };