MAVLINKHUD

Dijkstra Path Planning

Executive Summary

While BendyRuler is great for local, reactive avoidance, it can get stuck in "cul-de-sacs" (U-shaped obstacles). Dijkstra's Algorithm solves this by building a global map of the known fence and finding the mathematically optimal path around it.

It is computationally expensive but guarantees a solution if one exists within the fence boundaries.

Theory & Concepts

1. The Visibility Graph

To use Dijkstra on a continuous 2D plane, ArduPilot discretizes the world into a Visibility Graph.

  • Nodes: The vertices of all Inclusion and Exclusion polygons (plus a safety margin).
  • Edges: A line connects two nodes if the path between them does not intersect any fence line.
  • Logic: The drone can fly safely along any edge in the graph.

2. The Shortest Path

Dijkstra's algorithm explores the graph starting from the vehicle's position (Source).

  1. Assign tentative distance values to every node (0 for Source, Infinity for others).
  2. Visit the unvisited node with the smallest tentative distance.
  3. Calculate distances to its neighbors.
  4. Repeat until the Destination node is visited.

Codebase Investigation

1. Building the Graph: create_fence_visgraph()

Located in libraries/AC_Avoidance/AP_OADijkstra.cpp.

  • It expands the user-defined fence polygons by OA_MARGIN_MAX.
  • It iterates through every pair of points ($N^2$ complexity) to check for line-of-sight connectivity using intersects_fence().

2. The Solver: calc_shortest_path()

Located in libraries/AC_Avoidance/AP_OADijkstra.cpp.

  • Runs the standard Dijkstra loop.
  • Output: A list of waypoints (_shortest_path) that the vehicle must follow to clear the obstacle.

3. Dynamic Updates

Unlike a static map, the graph must be rebuilt if the destination changes or if the fence is updated via MAVLink. This is a heavy operation, often causing a momentary loop delay on slower processors (F4).

Source Code Reference

Practical Guide: When to use Dijkstra?

1. Complex Geo-Fencing

If you have a complex Exclusion Zone (e.g., a "Keep Out" zone for a building) and you want the drone to fly around it automatically during a mission, Dijkstra is the only option. BendyRuler might just stop at the edge.

2. Performance Limits

  • Point Limit: ArduPilot limits the total fence points to ~100 to keep calculation time reasonable.
  • Hardware: Recommended only for H7 boards (Cube Orange, Matek H743). On F4 boards, expect sluggishness during path recalculation.

3. "Dijkstra Failed" Errors

If you see this error:

  • Cause: The destination is inside an exclusion zone or outside an inclusion zone.
  • Fix: Ensure your WP mission is valid relative to the fence before uploading. (OA_TYPE = 2 enables Dijkstra).

How To: Setup Complex Geo-Fencing for Dijkstra

Dijkstra excels when you have "No Fly Zones" (NFZ) inside your mission area, like a building or a tree cluster.

1. Enable Dijkstra

  • OA_TYPE = 2 (Dijkstra).

2. Draw the Fence

  1. Open Mission Planner > Plan.
  2. Inclusion Zone: Draw a large Polygon Fence around the entire flight field. Right-click -> GeoFence -> Inclusion.
  3. Exclusion Zone: Draw a smaller polygon around the obstacle. Right-click -> GeoFence -> Exclusion.
  4. Upload: Click "Write GeoFence".

3. Plan a Mission

  1. Place Waypoint 1 on one side of the Exclusion Zone.
  2. Place Waypoint 2 on the opposite side.
  3. Do not place intermediate waypoints around the obstacle; Dijkstra will do that for you.

Result: When you switch to Auto, the drone will fly to WP1, then calculate a path around the Exclusion Zone (hugging the edge by OA_MARGIN_MAX) to reach WP2. You will see "OA: Path found" message on the HUD.