PositionSmoothing: constrain L1 point to line segment length

This additional logic handles edge cases where we are either behind the previous_setpoint, or ahead of the current_setpoint.

src/lib/motion_planning/PositionSmoothing.cpp

@@ -141,13 +141,8 @@ float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const
 
 const Vector3f PositionSmoothing::_getCrossingPoint(const Vector3f &position, const Vector3f(&waypoints)[3]) const
 {
-	const auto &target = waypoints[1];
-
-	if (!_isTurning(target)) {
-		return target;
-	}
-
-	// Get the crossing point using L1-style guidance
+	// Always use L1 guidance to handle edge cases (vehicle behind previous waypoint, etc.)
+	// The L1 logic will clamp to the segment bounds and handle straight-line cases appropriately
 	return _getL1Point(position, waypoints);
 }
 
@@ -157,8 +152,9 @@ const Vector3f PositionSmoothing::_getL1Point(const Vector3f &position, const Ve
 				_trajectory[2].getCurrentPosition());
 	const Vector3f u_prev_to_target = (waypoints[1] - waypoints[0]).unit_or_zero();
 	const Vector3f prev_to_pos(pos_traj - waypoints[0]);
-	const Vector3f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target));
-	const Vector3f closest_pt = waypoints[0] + prev_to_closest;
+	const float projection = prev_to_pos * u_prev_to_target;
+
+	const Vector3f closest_pt = waypoints[0] + u_prev_to_target * projection;
 
 	// Compute along-track error using L1 distance and cross-track error
 	const float crosstrack_error = (closest_pt - pos_traj).length();
@@ -171,8 +167,12 @@ const Vector3f PositionSmoothing::_getL1Point(const Vector3f &position, const Ve
 		alongtrack_error = sqrtf(l1 * l1 - crosstrack_error * crosstrack_error);
 	}
 
+	// Clamp the final point to the line segment bounds
+	const float segment_length = (waypoints[1] - waypoints[0]).length();
+	const float constrained_projection = math::constrain(projection + alongtrack_error, 0.0f, segment_length);
+
 	// Position of the point on the line where L1 intersect the line between the two waypoints
-	return closest_pt + alongtrack_error * u_prev_to_target;
+	return waypoints[0] + u_prev_to_target * constrained_projection;
 }
 
 const Vector3f PositionSmoothing::_generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3],

src/modules/navigator/mission.cpp

@@ -271,10 +271,41 @@ void Mission::setActiveMissionItems()
 		handleVtolTransition(new_work_item_type, next_mission_items, num_found_items);
 	}
 
-	// Only set the previous position item if the current one really changed
 	if ((_work_item_type != WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) &&
 	    !position_setpoint_equal(&pos_sp_triplet->current, &current_setpoint_copy)) {
-		pos_sp_triplet->previous = current_setpoint_copy;
+
+		// Check if previous waypoint is invalid (e.g., after mode switch / triplet reset)
+		// and we are continuing with a previous mission
+		// In that case, load the actual previous mission waypoint for proper line-following
+		if (!pos_sp_triplet->previous.valid && _mission.current_seq > 0) {
+			int32_t previous_position_index;
+			size_t num_found{0};
+
+			getPreviousPositionItems(_mission.current_seq, &previous_position_index, num_found, 1U);
+
+			if (num_found == 1U) {
+				mission_item_s previous_mission_item;
+				bool success = _dataman_cache.loadWait(mission_dataman_id, previous_position_index,
+								       reinterpret_cast<uint8_t *>(&previous_mission_item),
+								       sizeof(previous_mission_item), MAX_DATAMAN_LOAD_WAIT);
+
+				if (success) {
+					mission_item_to_position_setpoint(previous_mission_item, &pos_sp_triplet->previous);
+
+				} else {
+					// Fallback to standard behavior if load fails
+					pos_sp_triplet->previous = current_setpoint_copy;
+				}
+
+			} else {
+				// No previous position waypoint found, use standard behavior
+				pos_sp_triplet->previous = current_setpoint_copy;
+			}
+
+		} else {
+			// Normal progression: use old current as previous (standard behavior)
+			pos_sp_triplet->previous = current_setpoint_copy;
+		}
 	}
 
 	issue_command(_mission_item);