Navigator: Add ability to wait for a position gate with executing the mission

This enables the navigator to wait for a specific gate coordinate to pass orthogonally to the current trajectory. This is particularly helpful for payload / camera handling in missions and avoids a dependency of payload handling on navigation waypoints.

src/modules/navigator/mission.cpp

@@ -588,11 +588,14 @@ Mission::set_mission_items()
 
 	/* mission item that comes after current if available */
 	struct mission_item_s mission_item_next_position;
+	struct mission_item_s mission_item_next_next_position;
 	bool has_next_position_item = false;
+	bool has_next_next_position_item = false;
 
 	work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 
-	if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item)) {
+	if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item,
+					 &mission_item_next_next_position, &has_next_next_position_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_MISSION) {
 			mavlink_log_info(_navigator->get_mavlink_log_pub(),
@@ -796,9 +799,16 @@ Mission::set_mission_items()
 
 					set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
 
-					/* set position setpoint to target during the transition */
-					// TODO: if has_next_position_item and use_next set next, or if use_heading set generated
-					generate_waypoint_from_heading(&pos_sp_triplet->current, _mission_item.yaw);
+					if (has_next_position_item) {
+						/* got next mission item, update setpoint triplet */
+						mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->current);
+
+					} else {
+						_mission_item.yaw = _navigator->get_global_position()->yaw;
+
+						/* set position setpoint to target during the transition */
+						generate_waypoint_from_heading(&pos_sp_triplet->current, _mission_item.yaw);
+					}
 				}
 
 				/* takeoff completed and transitioned, move to takeoff wp as fixed wing */
@@ -1033,16 +1043,32 @@ Mission::set_mission_items()
 				break;
 			}
 		}
+
+		if (_mission_item.nav_cmd == NAV_CMD_CONDITION_GATE) {
+			_mission_item.autocontinue = true;
+			_mission_item.time_inside = 0;
+		}
 	}
 
 	/*********************************** set setpoints and check next *********************************************/
 
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
-	/* set current position setpoint from mission item (is protected against non-position items) */
-	if (new_work_item_type != WORK_ITEM_TYPE_PRECISION_LAND) {
-		mission_apply_limitation(_mission_item);
-		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+	if (item_contains_gate(&_mission_item)) {
+		if (has_next_position_item) {
+			/* we have a new position item so set previous position setpoint to current */
+			set_previous_pos_setpoint();
+			/* got next mission item, update setpoint triplet */
+			mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->current);
+		}
+
+	} else {
+		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+		/* set current position setpoint from mission item (is protected against non-position items) */
+		if (new_work_item_type != WORK_ITEM_TYPE_PRECISION_LAND) {
+			mission_apply_limitation(_mission_item);
+			mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+		}
 	}
 
 	/* only set the previous position item if the current one really changed */
@@ -1071,21 +1097,32 @@ Mission::set_mission_items()
 		set_current_mission_item();
 	}
 
-	if (_mission_item.autocontinue && get_time_inside(_mission_item) < FLT_EPSILON) {
-		/* try to process next mission item */
-		if (has_next_position_item) {
+	if (item_contains_gate(&_mission_item)) {
+		if (has_next_next_position_item) {
 			/* got next mission item, update setpoint triplet */
 			mission_apply_limitation(mission_item_next_position);
 			mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next);
 
 		} else {
-			/* next mission item is not available */
 			pos_sp_triplet->next.valid = false;
 		}
 
 	} else {
-		/* vehicle will be paused on current waypoint, don't set next item */
-		pos_sp_triplet->next.valid = false;
+		if (_mission_item.autocontinue && get_time_inside(_mission_item) < FLT_EPSILON) {
+			/* try to process next mission item */
+			if (has_next_position_item) {
+				/* got next mission item, update setpoint triplet */
+				mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->next);
+
+			} else {
+				/* next mission item is not available */
+				pos_sp_triplet->next.valid = false;
+			}
+
+		} else {
+			/* vehicle will be paused on current waypoint, don't set next item */
+			pos_sp_triplet->next.valid = false;
+		}
 	}
 
 	/* Save the distance between the current sp and the previous one */
@@ -1430,8 +1467,9 @@ Mission::do_abort_landing()
 }
 
 bool
-Mission::prepare_mission_items(mission_item_s *mission_item,
-			       mission_item_s *next_position_mission_item, bool *has_next_position_item)
+Mission::prepare_mission_items(struct mission_item_s *mission_item,
+			       struct mission_item_s *next_position_mission_item, bool *has_next_position_item,
+			       struct mission_item_s *next_next_position_mission_item, bool *has_next_next_position_item)
 {
 	*has_next_position_item = false;
 	bool first_res = false;
@@ -1447,18 +1485,29 @@ Mission::prepare_mission_items(mission_item_s *mission_item,
 
 		/* trying to find next position mission item */
 		while (read_mission_item(offset, next_position_mission_item)) {
-
-			if (item_contains_position(*next_position_mission_item)) {
-				*has_next_position_item = true;
-				break;
-			}
-
 			if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
 				offset--;
 
 			} else {
 				offset++;
+			}
 
+			if (item_contains_position(*next_position_mission_item)) {
+				*has_next_position_item = true;
+				break;
+			}
+		}
+
+		if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_REVERSE &&
+			next_next_position_mission_item && has_next_next_position_item) {
+			/* trying to find next next position mission item */
+			while (read_mission_item(onboard, offset, next_next_position_mission_item)) {
+				offset++;
+
+				if (item_contains_position(next_next_position_mission_item)) {
+					*has_next_next_position_item = true;
+					break;
+				}
 			}
 		}
 	}

src/modules/navigator/mission.h

@@ -177,7 +177,8 @@ private:
 	 * @return true if current mission item available
 	 */
 	bool prepare_mission_items(mission_item_s *mission_item,
-				   mission_item_s *next_position_mission_item, bool *has_next_position_item);
+				   mission_item_s *next_position_mission_item, bool *has_next_position_item,
+				   mission_item_s *next_next_position_mission_item = nullptr, bool *has_next_next_position_item = nullptr);
 
 	/**
 	 * Read current (offset == 0) or a specific (offset > 0) mission item

src/modules/navigator/mission_block.cpp

@@ -282,6 +282,47 @@ MissionBlock::is_mission_item_reached()
 				}
 			}
 
+		} else if (_mission_item.nav_cmd == NAV_CMD_CONDITION_GATE) {
+
+			// default to true as a number of geometric corner cases
+			// might prevent reaching the gate - we only set this to false
+			// if we are able to show that the gate conditions are feasible
+			bool gate_reached = true;
+
+			struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
+
+			// if either is not valid the gate is reached
+			if (curr_sp->valid) {
+
+				// zero is the current gate position
+				matrix::Vector2f gate_sp(0, 0);
+				struct map_projection_reference_s ref_pos;
+				map_projection_init(&ref_pos, _mission_item.lat, _mission_item.lon);
+
+				matrix::Vector2f gate_to_curr_sp;
+				map_projection_project(&ref_pos, curr_sp->lat, curr_sp->lon, &gate_to_curr_sp(0), &gate_to_curr_sp(1));
+				matrix::Vector2f vehicle_pos;
+				map_projection_project(&ref_pos, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+						       &vehicle_pos(0), &vehicle_pos(1));
+				float res = vehicle_pos.dot(gate_to_curr_sp.normalized());
+
+				// if the dot product (projected vector) is positive, then
+				// the current position is between the gate position and the
+				// next waypoint
+				if (res >= 0) {
+					gate_reached = true;
+
+				} else {
+					gate_reached = false;
+				}
+			}
+
+			if (gate_reached) {
+				_waypoint_position_reached = true;
+				_waypoint_yaw_reached = true;
+				_time_wp_reached = now;
+			}
+
 		} else if (_mission_item.nav_cmd == NAV_CMD_DELAY) {
 			_waypoint_position_reached = true;
 			_waypoint_yaw_reached = true;
@@ -428,12 +469,9 @@ MissionBlock::reset_mission_item_reached()
 void
 MissionBlock::issue_command(const mission_item_s &item)
 {
-	if (item_contains_position(item)) {
-		return;
-	}
-
-	// NAV_CMD_DO_LAND_START is only a marker
-	if (item.nav_cmd == NAV_CMD_DO_LAND_START) {
+	if (item_contains_position(item)
+	    || item_contains_gate(item)
+	    || item_contains_marker(item)) {
 		return;
 	}
 
@@ -509,7 +547,19 @@ MissionBlock::item_contains_position(const mission_item_s &item)
 }
 
 bool
-MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp)
+MissionBlock::item_contains_gate(const struct mission_item_s *item)
+{
+	return item->nav_cmd == NAV_CMD_CONDITION_GATE;
+}
+
+bool
+MissionBlock::item_contains_marker(const struct mission_item_s *item)
+{
+	return item->nav_cmd == NAV_CMD_DO_LAND_START;
+}
+
+bool
+MissionBlock::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp)
 {
 	/* don't change the setpoint for non-position items */
 	if (!item_contains_position(item)) {

src/modules/navigator/mission_block.h

@@ -67,7 +67,26 @@ public:
 	MissionBlock(const MissionBlock &) = delete;
 	MissionBlock &operator=(const MissionBlock &) = delete;
 
-	static bool item_contains_position(const mission_item_s &item);
+	/**
+	 * Check if the mission item contains a navigation position
+	 *
+	 * @return false if mission item should not be part of the trajectory
+	 */
+	static bool item_contains_position(const mission_item_s *item);
+
+	/**
+	 * Check if the mission item contains a gate condition
+	 *
+	 * @return true if mission item is neither a position nor a command
+	 */
+	static bool item_contains_gate(const struct mission_item_s *item);
+
+	/**
+	 * Check if the mission item contains a marker
+	 *
+	 * @return true if mission item is neither a position nor a command
+	 */
+	static bool item_contains_marker(const struct mission_item_s *item);
 
 protected:
 	/**

src/modules/navigator/mission_feasibility_checker.cpp

@@ -242,6 +242,7 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
 		    missionitem.nav_cmd != NAV_CMD_VTOL_TAKEOFF &&
 		    missionitem.nav_cmd != NAV_CMD_VTOL_LAND &&
 		    missionitem.nav_cmd != NAV_CMD_DELAY &&
+		    missionitem.nav_cmd != NAV_CMD_CONDITION_GATE &&
 		    missionitem.nav_cmd != NAV_CMD_DO_JUMP &&
 		    missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED &&
 		    missionitem.nav_cmd != NAV_CMD_DO_SET_HOME &&

src/modules/navigator/navigation.h

@@ -94,6 +94,7 @@ enum NAV_CMD {
 	NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION = 5002,
 	NAV_CMD_FENCE_CIRCLE_INCLUSION = 5003,
 	NAV_CMD_FENCE_CIRCLE_EXCLUSION = 5004,
+	NAV_CMD_CONDITION_GATE = 4501,
 	NAV_CMD_INVALID = UINT16_MAX /* ensure that casting a large number results in a specific error */
 };