Navigator: for loiter position acceptance, use L1 distance and loiter radius

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

src/modules/navigator/mission_block.cpp

@@ -197,12 +197,13 @@ MissionBlock::is_mission_item_reached()
 			 * coordinates with a radius equal to the loiter_radius field. It is not flying
 			 * through the waypoint center.
 			 * Therefore the item is marked as reached once the system reaches the loiter
-			 * radius (+ some margin). Time inside and turn count is handled elsewhere.
+			 * radius + L1 distance. 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();
 
-			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(radius) * 1.2f)
+			// check if within loiter radius around wp, if yes then set altitude sp to mission item
+			if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius))
 			    && dist_z <= _navigator->get_altitude_acceptance_radius()) {
 
 				_waypoint_position_reached = true;
@@ -228,7 +229,8 @@ MissionBlock::is_mission_item_reached()
 						_navigator->get_global_position()->alt,
 						&dist_xy, &dist_z);
 
-				if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
+				// check if within loiter radius around wp, if yes then set altitude sp to mission item
+				if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius))
 				    && dist_z <= _navigator->get_default_altitude_acceptance_radius()) {
 
 					// now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item
@@ -236,7 +238,7 @@ MissionBlock::is_mission_item_reached()
 					_navigator->set_position_setpoint_triplet_updated();
 				}
 
-			} else if (dist >= 0.f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
+			} else if (dist >= 0.f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius))
 				   && dist_z <= _navigator->get_altitude_acceptance_radius()) {
 
 				_waypoint_position_reached = true;
@@ -298,13 +300,9 @@ MissionBlock::is_mission_item_reached()
 			_time_wp_reached = now;
 
 		} else {
-			/* for normal mission items used their acceptance radius */
-			float mission_acceptance_radius = _navigator->get_acceptance_radius(_mission_item.acceptance_radius);
+			/*normal mission items */
 
-			/* if set to zero use the default instead */
-			if (mission_acceptance_radius < NAV_EPSILON_POSITION) {
-				mission_acceptance_radius = _navigator->get_acceptance_radius();
-			}
+			float mission_acceptance_radius = _navigator->get_acceptance_radius();
 
 			/* for vtol back transition calculate acceptance radius based on time and ground speed */
 			if (_mission_item.vtol_back_transition

src/modules/navigator/navigator.h

@@ -246,15 +246,6 @@ public:
 	 */
 	void		set_cruising_throttle(float throttle = NAN) { _mission_throttle = throttle; }
 
-	/**
-	 * Get the acceptance radius given the mission item preset radius
-	 *
-	 * @param mission_item_radius the radius to use in case the controller-derived radius is smaller
-	 *
-	 * @return the distance at which the next waypoint should be used
-	 */
-	float		get_acceptance_radius(float mission_item_radius);
-
 	/**
 	 * Get the yaw acceptance given the current mission item
 	 *

src/modules/navigator/navigator_main.cpp

@@ -873,12 +873,6 @@ Navigator::get_default_acceptance_radius()
 	return _param_nav_acc_rad.get();
 }
 
-float
-Navigator::get_acceptance_radius()
-{
-	return get_acceptance_radius(_param_nav_acc_rad.get());
-}
-
 float
 Navigator::get_default_altitude_acceptance_radius()
 {
@@ -996,24 +990,17 @@ Navigator::get_cruising_throttle()
 }
 
 float
-Navigator::get_acceptance_radius(float mission_item_radius)
+Navigator::get_acceptance_radius()
 {
-	float radius = mission_item_radius;
-
-	// XXX only use navigation capabilities for now
-	// when in fixed wing mode
-	// this might need locking against a commanded transition
-	// so that a stale _vstatus doesn't trigger an accepted mission item.
-
-	const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();
+	if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+		// return the value specified in the parameter NAV_ACC_RAD
+		return get_default_acceptance_radius();
 
-	if (_vstatus.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
-	    && (pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp)
-	    && pos_ctrl_status.acceptance_radius > radius) {
-		radius = pos_ctrl_status.acceptance_radius;
+	} else {
+		// return the max of NAV_ACC_RAD and the controller acceptance radius (e.g. L1 distance)
+		const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();
+		return math::max(pos_ctrl_status.acceptance_radius, get_default_acceptance_radius());
 	}
-
-	return radius;
 }
 
 float