FlightTaskAutoLine: legacy support velocity ramp at low altitude

src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp

@@ -55,6 +55,14 @@ bool FlightTaskAutoLine::update()
 	bool follow_line = _type == WaypointType::loiter || _type == WaypointType::position;
 	bool follow_line_prev = _type_previous == WaypointType::loiter || _type_previous == WaypointType::position;
 
+	// Altitude above ground from home or if not present
+	// the negation of the current position
+	_alt_above_ground = -_position(2);
+
+	if (_sub_home_position->get().valid_alt) {
+		_alt_above_ground = -_position(2) + _sub_home_position->get().z;
+	}
+
 	// 1st time that vehicle starts to follow line. Reset all setpoints to current vehicle state.
 	if (follow_line && !follow_line_prev) {
 		_reset();
@@ -380,9 +388,16 @@ void FlightTaskAutoLine::_generateAltitudeSetpoints()
 		// check sign
 		const bool flying_upward = _destination(2) < _position(2);
 
+
+		// limit vertical downwards speed (positive z) close to ground
+		// for now we use the altitude above home and assume that we want to land at same height as we took off
+		float vel_limit = math::gradual(_alt_above_ground,
+						_slow_land_alt2.get(), _slow_land_alt1.get(),
+						_land_speed.get(), _vel_max_down.get());
+
 		// Speed at threshold is by default maximum speed. Threshold defines
 		// the point in z at which vehicle slows down to reach target altitude.
-		float speed_sp = (flying_upward) ? _vel_max_up.get() : _vel_max_down.get();
+		float speed_sp = (flying_upward) ? _vel_max_up.get() : vel_limit;
 
 		// Target threshold defines the distance to target(2) at which
 		// the vehicle starts to slow down to approach the target smoothly.
@@ -493,3 +508,11 @@ float FlightTaskAutoLine::_getVelocityFromAngle(const float angle)
 	// speed_close needs to be in between max and min
 	return math::constrain(speed_close, min_cruise_speed, _mc_cruise_speed);
 }
+
+void FlightTaskAutoLine::updateParams()
+{
+	FlightTaskAuto::updateParams();
+
+	// make sure that alt1 is above alt2
+	_slow_land_alt1.set(math::max(_slow_land_alt1.get(), _slow_land_alt2.get()));
+}

src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp

@@ -55,6 +55,7 @@ protected:
 	matrix::Vector3f _destination{}; /**< Current target. Is not necessarily the same as triplet target. */
 	matrix::Vector3f _origin{}; /**< Previous waypoint. Is not necessarily the same as triplet previous. */
 	float _speed_at_target = 0.0f; /**< Desired velocity at target. */
+	float _alt_above_ground{0.0f}; /**< If home provided, then it is altitude above home, otherwise it is altitude above local position reference. */
 
 	enum class State {
 		offtrack, /**< Vehicle is more than cruise speed away from track */
@@ -74,7 +75,9 @@ protected:
 					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _acc_max_down,
 					(ParamFloat<px4::params::MPC_CRUISE_90>) _cruise_speed_90,
 					(ParamFloat<px4::params::NAV_ACC_RAD>) _nav_rad,
-					(ParamFloat<px4::params::MIS_YAW_ERR>) _mis_yaw_error
+					(ParamFloat<px4::params::MIS_YAW_ERR>) _mis_yaw_error,
+					(ParamFloat<px4::params::MPC_LAND_ALT1>) _slow_land_alt1,
+					(ParamFloat<px4::params::MPC_LAND_ALT2>) _slow_land_alt2
 				       )
 
 	void _generateIdleSetpoints();
@@ -85,6 +88,7 @@ protected:
 	void _generateSetpoints(); /**< Generate velocity and position setpoint for following line. */
 	void _generateAltitudeSetpoints(); /**< Generate velocity and position setpoints for following line along z. */
 	void _generateXYsetpoints(); /**< Generate velocity and position setpoints for following line along xy. */
+	void updateParams() override; /**< See ModuleParam class */
 
 private:
 	float _getVelocityFromAngle(const float angle); /** Computes the speed at target depending on angle. */