Path Management Architecture

ย 

ย Problem Statement

ย Scope

Must have:

  • Support various output/input requirements from AM, CV

    • Heading vector, roll/pitch/yaw

    • List of waypoints, one waypoint at a time for path following

    • relative directions, etc for landing

  • Auto takeoff landing

  • support for various flight modes

  • support for different models (fixed wing, quad, etc)

Must have:

  • Support various output/input requirements from AM, CV

    • Heading vector, roll/pitch/yaw

    • List of waypoints, one waypoint at a time for path following

    • relative directions, etc for landing

  • Auto takeoff landing

  • support for various flight modes

  • support for different models (fixed wing, quad, etc)

Nice to have:

  • pre programmed โ€œhomeโ€œ waypoint to return to

  • hover/orbit feature around waypoint

Not in scope:

  • ย 

ย 

ย Timeline

ย 

Milestones have been broken down into sub-tasks with their own deadline

More info later

July 15 - PM arch finalized (PM states, unit testing, etc)

July 22 - 1st ver fixed wing flight (including unit testing), and auto takeoff for fixed wing (orbit feature if time permits)

July 29 - Unit testing for auto take off

Sept 3 - 2nd revision for fixed wing flight, begin dev for quad path following

Sept 10 - Takeoff done

Sept 15 - Begin fixed wing testing, begin quad takeoff/landing, finish fixed wing landing

ย Milestones and deadlines

Milestone

Owner

Deadline

Status

Milestone

Owner

Deadline

Status

ย 

ย 

ย 

ย 

ย 

ย 

ย 

ย 

ย 

ย Data From TM

The following defines the structure and data that is sent by TM. These follow MAVLink guidelines, as referenced by the MAVLink Developer Guide Mission Protocol ยท MAVLink Developer Guide .

Field Name

Type

Description

Field Name

Type

Description

command

uint16_t

Command id

param1

float

Param #1.

param2

float

Param #2.

param3

float

Param #3.

param4

float

Param #4.

param5 (x)

int32_t

X coordinate (local frame) or latitude (global frame) for navigation commands (otherwise Param #5).

param6 (y)

int32_t

Y coordinate (local frame) or longitude (global frame) for navigation commands (otherwise Param #6).

param7 (z)

float

Z coordinate (local frame) or altitude (global - relative or absolute, depending on frame) (otherwise Param #7).

seq

uint16_t

Sequence number for item within mission (indexed from 0).

frame

uint8_t

The coordinate system of the waypoint.
ArduPilot and PX4 both only support global frames in MAVLink commands (local frames may be supported if the same command is sent via the command protocol).

autocontinue

uint8_t

Autocontinue to next waypoint when the command completes.

The above data structure is just a template for what PM is expecting to receive from TM. The specific commands and its fields that are populated are below:

Waypoint Commands (MAV_CMD_NAV_WAYPOINT)

Loiter Commands (MAV_CMD_NAV_LOITER_*)

Specifically for Fixed Wing: for MAV_CMD_NAV_LOITER_TIME and MAV_CMD_NAV_LOITER_TURNS

More information can be found here: https://mavlink.io/en/services/mission.html#loiter_commands

Go Home Commands (MAV_CMD_NAV_RETURN_TO_LAUNCH)

Land Commands (MAV_CMD_NAV_LAND)

Go Home Commands (MAV_CMD_NAV_TAKEOFF)

Follow Commands

Note: although mavlink provides MAV_CMD_NAV_FOLLOW for current position, zero pilot is using its own gps driver and its own implementation (which is why this manager exists)

ย Data To AM (TBD)

The current PM utilizes XYZ coordinate system (and can switch to longitude/latitude/altitude if needed). AM requested that the information sent to AM be yaw, pitch, raw, and throttle. The exact method and data type these will be sent to AM is yet to be determined since the way AM will handle inputs are also yet to be determined (for example, do they want 1-100 value based on ppm like the controller, or direct degrees of yaw/pitch/raw/throttle? How should PM encode the data?)

Perhaps this may be of use: https://mavlink.io/en/services/manual_control.html

ย Reference materials

PM 3.0 https://github.com/UWARG/ZeroPilot-SW-3/tree/PM/New_Arch

PM 3.0 Waypoint Following https://github.com/UWARG/ZeroPilot-SW-3/tree/PM/Waypoint_Following

Old PM https://github.com/antholuo/ZeroPilot-SW-ALuo/tree/8d191f1c948e1dcf5a829703f4efba658a8a042c

Mavlink Data Mission Protocol ยท MAVLink Developer Guide

Current state of PM

ย 

Current state of PM (2024-06-16)

In short, this is how PM works: Telemetry manager, using Mavlink, sends flight state and waypoints to Path manager. PM stores those waypoints and fetch sensor fusion data - to get position and direction (vector) to steer through a calculation process similar to that of Waypoint manager logics from Zeropilot 1.0. Regardless of whether TM sends the waypoints or not, PM will fetch sensor fusion data and carve out the path at some interval. (In case PM receives no waypoint, this will be garbage value.)

From this, Attitude manager gets controller data; get vectors and map it into a value in AM so it could determine where to turn (Pitch, Roll, Yaw, etcโ€ฆ). We repeat this process until the flight is done.ย 

In short, PM stores waypoints received from TM and gives direction vectors to AM. How this direction is formatted is up to AM.

Problem

We have our algorithm implemented by Liang for both takeoff and landing. Keep in mind the takeoff and landing algorithm can only be applied to quad. Cruising algorithm can only be applied to fixed wings. Quad and fixed wings both require different algorithms due to their structure and flight style.

The team was able to store path and waypoints in a buffer properly. Unit test passed. However, calculating the next waypoint is odd. Somethingโ€™s wrong with getting the direction (waypoint follows). This has been unit tested and failed. Requires debugging. To be exact, CruisingStatemanagerTest.cpp math is wonky at line 40.

Another problem is that the outputs that the Telemetry Manager sends and inputs that Attitude manager wants keep changing. They never settled on one solid input/output, so PM had to keep adjusting for that.

Next Step

  • Confirm what input/output TM and AM wants and ensure that they will not change their mind.

  • Confirm if the Autonomy team already developed their own landing algorithm. We will not need a landing algorithm if they already have a method to land off.

  • Debug wonky math in the algorithm

  • Develop other states as needed.

+ I will add stakeholders here once confirmed