Path Management Architecture
- Neha Srivastava
- Ayoung Eun
- Liang Li
Problem Statement
Scope
Must have: |
|
|---|---|
Nice to have: |
|
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 |
|---|---|---|---|
| |||
|
|
|
|
|
|
|
|
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 Guide .
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. |
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
MAV_CMD_NAV_LOITER_TIME - Loiter at specified location for a given amount of time after reaching the location. https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_LOITER_UNLIM
MAV_CMD_NAV_LOITER_TURNS - Loiter at specified location for a given number of turns. https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_LOITER_TURNS
MAV_CMD_NAV_LOITER_TO_ALT - Loiter at specified location until desired altitude is reached.
MAV_CMD_NAV_LOITER_UNLIM - Loiter at specified location for an unlimited amount of time, yawing to face a given direction.https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_LOITER_TIME
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 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