FlightTaskFailsafe: comments and land with land speed if velocity in z is valid but altitude not

src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.cpp

@@ -41,7 +41,7 @@ bool FlightTaskFailsafe::activate()
 	bool ret = FlightTask::activate();
 	_position_setpoint = _position;
 	_velocity_setpoint *= 0.0f;
-	_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -MPC_THR_HOVER.get() * 0.5f);
+	_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -MPC_THR_HOVER.get() * 0.6f);
 	_yaw_setpoint = _yaw;
 	_yawspeed_setpoint = 0.0f;
 	return ret;
@@ -50,19 +50,24 @@ bool FlightTaskFailsafe::activate()
 bool FlightTaskFailsafe::update()
 {
 	if (PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1))) {
+		// stay at current position setpoint
 		_velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
 		_thrust_setpoint(0) = _thrust_setpoint(1) = 0.0f;
 
 	} else if (PX4_ISFINITE(_velocity(0)) && PX4_ISFINITE(_velocity(1))) {
+		// don't move along xy
 		_position_setpoint(0) = _position_setpoint(1) = NAN;
 		_thrust_setpoint(0) = _thrust_setpoint(1) = NAN;
 	}
 
 	if (PX4_ISFINITE(_position(2))) {
+		// stay at current altitude setpoint
 		_velocity_setpoint(2) = 0.0f;
 		_thrust_setpoint(2) = NAN;
 
 	} else if (PX4_ISFINITE(_velocity(2))) {
+		// land with landspeed
+		_velocity_setpoint(2) = MPC_LAND_SPEED.get();
 		_position_setpoint(2) = NAN;
 		_thrust_setpoint(2) = NAN;
 	}