mc_pos_control: ignore a NaN/inf position setpoint (#7186)

* mc_pos_control: ignore a NaN/inf position setpoint

This is a hotfix that prevents the position controller from trying to
do velocity control with a position setpoint that isn't valid in the
first place.

This is only a workaround, ideally the controls later should not scale
down throttle to the minimum just because the position setpoint is far
away if they still have valid setpoint in z.

* mc_pos_control: use PX4_ISFINITE and not isfinite

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -1693,8 +1693,16 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt)
 {
 	/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
 	if (_run_pos_control) {
-		_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
-		_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
+
+		// If for any reason, we get a NaN position setpoint, we better just stay where we are.
+		if (PX4_ISFINITE(_pos_sp(0)) && PX4_ISFINITE(_pos_sp(1))) {
+			_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
+			_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
+
+		} else {
+			_vel_sp(0) = 0.0f;
+			_vel_sp(1) = 0.0f;
+		}
 	}
 
 	limit_altitude();