mission block: use dist_xy instead of dist for horizontal pos error check

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -753,7 +753,6 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 			} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
 				// LOITER: use SETPOINT_TYPE_POSITION to get to SETPOINT_TYPE_LOITER
 				if ((dist >= 0.f)
-				    && (dist_z > 2.f * _param_fw_clmbout_diff.get())
 				    && (dist_xy > 2.f * math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius)))) {
 					// SETPOINT_TYPE_LOITER -> SETPOINT_TYPE_POSITION
 					position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;

src/modules/navigator/mission_block.cpp

@@ -199,10 +199,11 @@ MissionBlock::is_mission_item_reached()
 			 * Therefore the item is marked as reached once the system reaches the loiter
 			 * radius (+ some margin). Time inside and turn count is handled elsewhere.
 			 */
-			float radius = (fabsf(_mission_item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(_mission_item.loiter_radius) :
-				       _navigator->get_loiter_radius();
+			const float radius = (fabsf(_mission_item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(_mission_item.loiter_radius) :
+					     _navigator->get_loiter_radius();
+
+			if (dist >= 0.0f && dist_xy <= _navigator->get_acceptance_radius(fabsf(radius) * 1.2f)
 
-			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(radius) * 1.2f)
 			    && dist_z <= _navigator->get_altitude_acceptance_radius()) {
 
 				_waypoint_position_reached = true;
@@ -408,7 +409,9 @@ MissionBlock::is_mission_item_reached()
 					bearing += inner_angle;
 				}
 
-				// Replace current setpoint lat/lon with tangent coordinate
+				// set typ to position, will get set to loiter in the fw position controller once close
+				// and replace current setpoint lat/lon with tangent coordinate
+				curr_sp.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 				waypoint_from_heading_and_distance(curr_sp.lat, curr_sp.lon,
 								   bearing, fabsf(curr_sp.loiter_radius),
 								   &curr_sp.lat, &curr_sp.lon);
@@ -596,10 +599,8 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 			sp->alt = _navigator->get_global_position()->alt;
 		}
 
-	// fall through
-	case NAV_CMD_LOITER_TIME_LIMIT:
-
 	// FALLTHROUGH
+	case NAV_CMD_LOITER_TIME_LIMIT:
 	case NAV_CMD_LOITER_UNLIMITED:
 		sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
 		break;