PositionControl: tiny minimal thrust length

To be able to still infer the direction of the thrust vector we
limit it to a minimal length even if MPC_THR_MIN is set to zero.

Note: This is a hotfix for certain specific applications.
The direction of the thrust vector in this corner case is very
likely to get into the tilt limit which is generally undesired.

src/modules/mc_pos_control/PositionControl.cpp

@@ -265,6 +265,9 @@ void PositionControl::_velocityController(const float &dt)
 	float uMax = -_param_mpc_thr_min.get();
 	float uMin = -_param_mpc_thr_max.get();
 
+	// make sure there's always enough thrust vector length to infer the attitude
+	uMax = math::min(uMax, -10e-4f);
+
 	// Apply Anti-Windup in D-direction.
 	bool stop_integral_D = (thrust_desired_D >= uMax && vel_err(2) >= 0.0f) ||
 			       (thrust_desired_D <= uMin && vel_err(2) <= 0.0f);

src/modules/mc_pos_control/Utility/ControlMath.cpp

@@ -57,8 +57,7 @@ vehicle_attitude_setpoint_s thrustToAttitude(const Vector3f &thr_sp, const float
 
 	} else {
 		// no thrust, set Z axis to safe value
-		body_z.zero();
-		body_z(2) = 1.0f;
+		body_z = Vector3f(0.f, 0.f, 1.f);
 	}
 
 	// vector of desired yaw direction in XY plane, rotated by PI/2