FlightTaskManualAltitude: slow down when landing manually

src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -41,6 +41,19 @@
 
 using namespace matrix;
 
+bool FlightTaskManualAltitude::initializeSubscriptions(SubscriptionArray &subscription_array)
+{
+	if (!FlightTaskManual::initializeSubscriptions(subscription_array)) {
+		return false;
+	}
+
+	if (!subscription_array.get(ORB_ID(home_position), _sub_home_position)) {
+		return false;
+	}
+
+	return true;
+}
+
 bool FlightTaskManualAltitude::updateInitialize()
 {
 	bool ret = FlightTaskManual::updateInitialize();
@@ -252,6 +265,27 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
 	}
 }
 
+void FlightTaskManualAltitude::_respectGroundSlowdown()
+{
+	float dist_to_ground = NAN;
+
+	// if there is a valid distance to bottom or vertical distance to home
+	if (PX4_ISFINITE(_dist_to_bottom)) {
+		dist_to_ground = _dist_to_bottom;
+
+	} else if (_sub_home_position->get().valid_alt) {
+		dist_to_ground = -(_position(2) - _sub_home_position->get().z);
+	}
+
+	// limit downwards speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2
+	if (PX4_ISFINITE(dist_to_ground)) {
+		const float slowdown_limit = math::gradual(dist_to_ground,
+					     MPC_LAND_ALT2.get(), MPC_LAND_ALT1.get(),
+					     MPC_LAND_SPEED.get(), _constraints.speed_down);
+		_velocity_setpoint(2) = math::min(_velocity_setpoint(2), slowdown_limit);
+	}
+}
+
 void FlightTaskManualAltitude::_rotateIntoHeadingFrame(Vector2f &v)
 {
 	float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw;
@@ -305,6 +339,7 @@ void FlightTaskManualAltitude::_updateSetpoints()
 	_thrust_setpoint(2) = NAN;
 
 	_updateAltitudeLock();
+	_respectGroundSlowdown();
 }
 
 bool FlightTaskManualAltitude::update()