Navigator: Improve code readibility

This diff does not contain any functional changes but just makes the code more readable and adds comments.

src/modules/navigator/mission.cpp

@@ -512,7 +512,7 @@ Mission::update_mission()
 		// only warn if the check failed on merit
 		if ((int)_mission.count > 0) {
 			PX4_WARN("mission check failed");
-	}
+		}
 
 		// reset the mission
 		_mission.count = 0;
@@ -592,14 +592,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;
+	struct mission_item_s mission_item_after_next_position;
 	bool has_next_position_item = false;
-	bool has_next_next_position_item = false;
+	bool has_after_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,
-				  &mission_item_next_next_position, &has_next_next_position_item)) {
+				  &mission_item_after_next_position, &has_after_next_position_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_MISSION) {
 			mavlink_log_info(_navigator->get_mavlink_log_pub(),
@@ -1058,21 +1058,43 @@ Mission::set_mission_items()
 
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
+	// The logic in this section establishes the tracking between the current waypoint
+	// which we are approaching and the next waypoint, which will tell us in which direction
+	// we will change our trajectory right after reaching it.
+
+	// Because actions, gates and jump labels can be interleaved with waypoints,
+	// we are searching around the current mission item in the list to find the closest
+	// gate and the closest waypoint. We then store them separately.
+
+	// Check if the mission item is a gate
+	// along the current trajectory
 	if (item_contains_gate(_mission_item)) {
+
+		// The mission item is a gate, let's check if the next item in the list provides
+		// a position to go towards.
+
+		// TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320
 		if (has_next_position_item) {
-			/* got next mission item, update setpoint triplet */
+			// We have a position, convert it to the setpoint and update setpoint triplet
+			mission_apply_limitation(mission_item_next_position);
 			mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current);
 		}
 
+		// ELSE: The current position setpoint stays unchanged.
+
 	} else {
-		/* set current position setpoint from mission item (is protected against non-position items) */
+		// The mission item is not a gate, set the current position setpoint from mission item (is protected against non-position items)
+		// TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320
 		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);
 		}
+
+		// ELSE: The current position setpoint stays unchanged.
 	}
 
-	/* only set the previous position item if the current one really changed */
+	// Only set the previous position item if the current one really changed
+	// TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320
 	if ((_work_item_type != WORK_ITEM_TYPE_MOVE_TO_LAND) &&
 	    !position_setpoint_equal(&pos_sp_triplet->current, &current_setpoint_copy)) {
 		pos_sp_triplet->previous = current_setpoint_copy;
@@ -1098,8 +1120,12 @@ Mission::set_mission_items()
 		set_current_mission_item();
 	}
 
+	// If the mission item under evaluation contains a gate,
+	// then we need to check if we have a next position item so
+	// the controller can fly the correct line between the
+	// current and next setpoint
 	if (item_contains_gate(_mission_item)) {
-		if (has_next_next_position_item) {
+		if (has_after_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);
@@ -1470,7 +1496,7 @@ Mission::do_abort_landing()
 bool
 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)
+			       struct mission_item_s *after_next_position_mission_item, bool *has_after_next_position_item)
 {
 	*has_next_position_item = false;
 	bool first_res = false;
@@ -1500,13 +1526,13 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item,
 		}
 
 		if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_REVERSE &&
-		    next_next_position_mission_item && has_next_next_position_item) {
+		    after_next_position_mission_item && has_after_next_position_item) {
 			/* trying to find next next position mission item */
-			while (read_mission_item(offset, next_next_position_mission_item)) {
+			while (read_mission_item(offset, after_next_position_mission_item)) {
 				offset++;
 
-				if (item_contains_position(*next_next_position_mission_item)) {
+				if (item_contains_position(*after_next_position_mission_item)) {
-					*has_next_next_position_item = true;
+					*has_after_next_position_item = true;
 					break;
 				}
 			}

src/modules/navigator/mission_block.h

@@ -70,21 +70,21 @@ public:
 	/**
 	 * Check if the mission item contains a navigation position
 	 *
-	 * @return false if mission item should not be part of the trajectory
+	 * @return false if the mission item does not contain a valid position
 	 */
 	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
+	 * @return true if mission item is a gate
 	 */
 	static bool item_contains_gate(const mission_item_s &item);
 
 	/**
 	 * Check if the mission item contains a marker
 	 *
-	 * @return true if mission item is neither a position nor a command
+	 * @return true if mission item is a marker
 	 */
 	static bool item_contains_marker(const mission_item_s &item);