rtl_direct: only update destination if it's invalid or the position actually

changed

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.cpp

@@ -368,13 +368,18 @@ void GeofenceAvoidancePlanner::update_destination(const matrix::Vector2d &destin
 		return;
 	}
 
-	perf_begin(_update_destination_perf);
-
 	MapProjection ref{_reference(0), _reference(1)};
 	matrix::Vector2f dest_local;
 	ref.project(destination(0), destination(1), dest_local(0), dest_local(1));
 
 	const int dest_idx = _num_nodes - 1;
+
+	if (_destination_healthy && (dest_local - _graph_nodes[dest_idx].position).norm() < FLT_EPSILON) {
+		// no change in destination, skip the update
+		return;
+	}
+
+	perf_begin(_update_destination_perf);
 	_graph_nodes[dest_idx].position = dest_local;
 
 	// distances from each vertex to destination

src/modules/navigator/rtl_direct.cpp

@@ -74,6 +74,9 @@ void RtlDirect::on_activation()
 	_vehicle_status_sub.update();
 
 	_navigator->updateStartOfRTLPathPlanner(_navigator->getRtlPlanningStart());
+
+	// destination will only update if the location is different or the previous location was invalid
+	_navigator->updateDestinationOfRTLPathPlanner(matrix::Vector2d{_land_approach.lat, _land_approach.lon});
 	_geofence_aware_return_path = _navigator->planPath();
 
 	parameters_update();