mission_block: change loiter handling logic

- add ability of "heading wait" for NAV_CMD_LOITER_TIME_LIMIT
- For both LOITER_TIME and LOITER_TO_ALT in fixed-wing flight, unify logic:
--> reach position --> start loitering --> reach altitude --> start timer (if applicable)
	--> reach exit heading (if applicable) --> declare mission item reached

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

src/modules/mavlink/mavlink_mission.cpp

@@ -1640,6 +1640,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 
 		case NAV_CMD_LOITER_TIME_LIMIT:
 			mavlink_mission_item->param1 = mission_item->time_inside;
+			mavlink_mission_item->param2 = mission_item->force_heading;
 			mavlink_mission_item->param3 = mission_item->loiter_radius;
 			mavlink_mission_item->param4 = mission_item->loiter_exit_xtrack;
 			break;

src/modules/navigator/mission_block.cpp

@@ -141,9 +141,7 @@ MissionBlock::is_mission_item_reached()
 		float dist_xy = -1.0f;
 		float dist_z = -1.0f;
 
-		float mission_item_altitude_amsl = _mission_item.altitude_is_relative
-						   ? _mission_item.altitude + _navigator->get_home_position()->alt
-						   : _mission_item.altitude;
+		const float mission_item_altitude_amsl = get_absolute_altitude_for_item(_mission_item);
 
 		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, mission_item_altitude_amsl,
 				_navigator->get_global_position()->lat,
@@ -199,27 +197,20 @@ MissionBlock::is_mission_item_reached()
 			 * Therefore the item is marked as reached once the system reaches the loiter
 			 * 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();
 
 			// 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;
-
-			} else {
-				_time_first_inside_orbit = 0;
 			}
 
 		} else if (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) {
-
-			// NAV_CMD_LOITER_TO_ALT only uses mission item altitude once it's in the loiter
-			//  first check if the altitude setpoint is the mission setpoint
-			position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
+			// NAV_CMD_LOITER_TO_ALT only uses mission item altitude once it's in the loiter.
+			// First check if the altitude setpoint is the mission setpoint (that means that the loiter is not yet reached)
+			struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
 
 			if (fabsf(curr_sp->alt - mission_item_altitude_amsl) >= FLT_EPSILON) {
-				// check if the initial loiter has been accepted
 				dist_xy = -1.0f;
 				dist_z = -1.0f;
 
@@ -233,31 +224,16 @@ MissionBlock::is_mission_item_reached()
 				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
 					curr_sp->alt = mission_item_altitude_amsl;
+					curr_sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
 					_navigator->set_position_setpoint_triplet_updated();
 				}
 
 			} else if (dist >= 0.f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius))
 				   && dist_z <= _navigator->get_altitude_acceptance_radius()) {
+				// loitering, check if new altitude is reached, while still also having check on position
 
 				_waypoint_position_reached = true;
-
-				// set required yaw from bearing to the next mission item
-				if (_mission_item.force_heading) {
-					const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next;
-
-					if (next_sp.valid) {
-						_mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
-								    _navigator->get_global_position()->lon,
-								    next_sp.lat, next_sp.lon);
-
-						_waypoint_yaw_reached = false;
-
-					} else {
-						_waypoint_yaw_reached = true;
-					}
-				}
 			}
 
 		} else if (_mission_item.nav_cmd == NAV_CMD_CONDITION_GATE) {
@@ -327,29 +303,25 @@ MissionBlock::is_mission_item_reached()
 			}
 		}
 
-		if (_waypoint_position_reached && !_waypoint_position_reached_previously) {
+		if (_waypoint_position_reached) {
 			// reached just now
 			_time_wp_reached = now;
 		}
+
+		// consider yaw reached for non-rotary wing vehicles (such as fixed-wing)
+		if (_navigator->get_vstatus()->vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+			_waypoint_yaw_reached = true;
+		}
 	}
 
-	/* Check if the waypoint and the requested yaw setpoint. */
+	/* Check if the requested yaw setpoint is reached (only for rotary wing flight). */
+
 	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
 
-		if ((_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
-		     && PX4_ISFINITE(_navigator->get_yaw_acceptance(_mission_item.yaw)))
-		    || ((_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) && _mission_item.force_heading
-			&& PX4_ISFINITE(_mission_item.yaw))) {
+		if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+		    && PX4_ISFINITE(_navigator->get_yaw_acceptance(_mission_item.yaw))) {
 
-			/* check course if defined only for rotary wing except takeoff */
-			float cog = (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) ?
-				    _navigator->get_local_position()->heading :
-				    atan2f(
-					    _navigator->get_local_position()->vy,
-					    _navigator->get_local_position()->vx
-				    );
-
-			float yaw_err = wrap_pi(_mission_item.yaw - cog);
+			const float yaw_err = wrap_pi(_mission_item.yaw - _navigator->get_local_position()->heading);
 
 			/* accept yaw if reached or if timeout is set in which case we ignore not forced headings */
 			if (fabsf(yaw_err) < _navigator->get_yaw_threshold()
@@ -374,14 +346,62 @@ MissionBlock::is_mission_item_reached()
 	/* Once the waypoint and yaw setpoint have been reached we can start the loiter time countdown */
 	if (_waypoint_position_reached && _waypoint_yaw_reached) {
 
-		if (_time_first_inside_orbit == 0) {
-			_time_first_inside_orbit = now;
-		}
+		bool time_inside_reached = false;
 
 		/* check if the MAV was long enough inside the waypoint orbit */
 		if ((get_time_inside(_mission_item) < FLT_EPSILON) ||
-		    (now - _time_first_inside_orbit >= (hrt_abstime)(get_time_inside(_mission_item) * 1e6f))) {
+		    (now - _time_wp_reached >= (hrt_abstime)(get_time_inside(_mission_item) * 1e6f))) {
+			time_inside_reached = true;
+		}
 
+		// check if heading for exit is reached (only applies for fixed-wing flight)
+		bool exit_heading_reached = false;
+
+		if (time_inside_reached) {
+
+			struct position_setpoint_s *curr_sp_new = &_navigator->get_position_setpoint_triplet()->current;
+			const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next;
+
+			/* enforce exit heading if in FW, the next wp is valid, the vehicle is currently loitering and either having force_heading set,
+			   or if loitering to achieve altitdue at a NAV_CMD_WAYPOINT */
+			const bool enforce_exit_heading = _navigator->get_vstatus()->vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+							  &&
+							  next_sp.valid &&
+							  curr_sp_new->type == position_setpoint_s::SETPOINT_TYPE_LOITER &&
+							  (_mission_item.force_heading || _mission_item.nav_cmd == NAV_CMD_WAYPOINT);
+
+			if (enforce_exit_heading) {
+
+
+				const float dist_current_next = get_distance_to_next_waypoint(curr_sp_new->lat, curr_sp_new->lon, next_sp.lat,
+								next_sp.lon);
+
+				float yaw_err = 0.0f;
+
+				if (dist_current_next >  1.2f * _navigator->get_loiter_radius()) {
+					// set required yaw from bearing to the next mission item
+					_mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
+							    _navigator->get_global_position()->lon,
+							    next_sp.lat, next_sp.lon);
+					const float cog = atan2f(_navigator->get_local_position()->vy, _navigator->get_local_position()->vx);
+					yaw_err = wrap_pi(_mission_item.yaw - cog);
+
+
+
+				}
+
+
+				if (fabsf(yaw_err) < 0.1f) { //accept heading for exit if below 0.1 rad error (5.7deg)
+					exit_heading_reached = true;
+				}
+
+			} else {
+				exit_heading_reached = true;
+			}
+		}
+
+		// set exit flight course to next waypoint
+		if (exit_heading_reached) {
 			position_setpoint_s &curr_sp = _navigator->get_position_setpoint_triplet()->current;
 			const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next;
 
@@ -412,14 +432,10 @@ MissionBlock::is_mission_item_reached()
 								   &curr_sp.lat, &curr_sp.lon);
 			}
 
-			return true;
+			return true; // mission item is reached
 		}
 	}
 
-	// all acceptance criteria must be met in the same iteration
-	_waypoint_position_reached_previously = _waypoint_position_reached;
-	_waypoint_position_reached = false;
-	_waypoint_yaw_reached = false;
 	return false;
 }
 
@@ -428,7 +444,6 @@ MissionBlock::reset_mission_item_reached()
 {
 	_waypoint_position_reached = false;
 	_waypoint_yaw_reached = false;
-	_time_first_inside_orbit = 0;
 	_time_wp_reached = 0;
 }
 
@@ -586,7 +601,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 	case NAV_CMD_LOITER_TO_ALT:
 
 		// initially use current altitude, and switch to mission item altitude once in loiter position
-		if (_navigator->get_loiter_min_alt() > 0.f) { // ignore _param_loiter_min_alt if smaller than 0 (-1)
+		if (_navigator->get_loiter_min_alt() > 0.f) { // ignore _param_loiter_min_alt if smaller than 0
 			sp->alt = math::max(_navigator->get_global_position()->alt,
 					    _navigator->get_home_position()->alt + _navigator->get_loiter_min_alt());
 
@@ -594,11 +609,10 @@ 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;
 

src/modules/navigator/mission_block.h

@@ -148,9 +148,7 @@ protected:
 
 	bool _waypoint_position_reached{false};
 	bool _waypoint_yaw_reached{false};
-	bool _waypoint_position_reached_previously{false};
 
-	hrt_abstime _time_first_inside_orbit{0};
 	hrt_abstime _action_start{0};
 	hrt_abstime _time_wp_reached{0};