Allan Su + Ben Carnes (will ping him when he gets access to Asana)
We are looking to research if Gordon’s vector implementation would still be valid, or should we come up with completely new code for Path Manager zp 3.5, Assuming that the input comes from FreeRTOS queue, and show our approach.
We will be assuming that the inputs will be FreeRTOS queues.
Helpful Source (Autonomous Team’s landing and takeoff code)
https://github.com/UWARG/pathing/tree/main
Previous research we could refer, and possibly extend from:
...
Current Method
-Using vectors and way points
-The current vector implementation of the drone relies on ideal conditions for takeoff.
-Does not seem to use strong feedback loop to compare current position to goal, does not respond dynamically
PID Control Loop
We can better handle a changing environment if we add in some PID concepts into the implementation. We can adjust gradually the trajectories and get smoother acceleration and deceleration during takeoff or landing. This can also improve deviations, undershoots/overshoots. Given a set of waypoints assumed to be in a queue, we compare current position to the next waypoint. The error between the two position can be used to make corrective changes.
Model Predicative Control
Existing Information:
Landing Procedure
...
Question : Does PM output speed to AM? Or does AM computes the speed
Speed should be controlled by AM.
Should still PM generate waypoints for landing/takeoff?
Yes, but it may change.
Ideas supposing we adapted the current code:
We likely will not need getRangeConstant which is used to define the targetSpeed of a VTOL. However, the target of a fixed wing craft is more so based on stage of take off or landing which is dependent mainly on stall speed, takeoff speed, landing speed and minimum climb speed.
For getTakeoffAltitudeTarget, we likely could exit take off or landing states by completing the final waypoints during takeoff and landing.
We need a horizontal component which would allow our drone to travel in oblique vectors
| Code Block |
|---|
desiredWaypoint.dist_forward = 0 |
If our drone is ever flying into cross wind, we may need to correct lateral movement to reach specified waypoints.
| Code Block |
|---|
desiredWaypoint.dist_right = 0; |
The current getSpeedTarget function is a exponential decay calculated using altitude function which is suitable to VTOL.
| Code Block |
|---|
speedTarget = MAX_SPEED * exp(-1.0 * (pow((currentAltitude - groundHeight - (TAKEOFF_TARGET / 2)), 2) / rangeConstant)); |
However, the speed of a fixed wing craft is largely independent of altitude. There is also different behaviors required at each stage in take off or landing. The big concern of a fixed wing craft is the stall speed and minimum climb speed. It may be better to have two separate getSpeedTarget functions for a take off and landing of a fixed wing craft. For takeoff, we could try for some high constant takeoff speed to minimize runway length.
Example speed logic in a getTakeoffTargetSpeed function
| Code Block |
|---|
double takeoffSpeed= stallSpeed * 1.2;
return takeoffSpeed; |
Since we need to change behaviors during takeoff, we could pass a queue of waypoint into the getTakeoffTargetSpeed function to determine what speed is optimal for what stage. Maybe switch statements based on stage? There could be better options but this is just a makeshift solution for now.
Useful Documents to look over:
https://link-springer-com.proxy.lib.uwaterloo.ca/article/10.1007/s10846-017-0512-y
Looking specifically at the middle level control and high level control section, there are several points of interest.
Landing
Slope Stage
To get a better landing algorithm, we may be interested in the section on Proportional Guidance Law on pg 626. We can create a waypoint from which we find the ideal glide path from the drone. We will need a waypoint as the touchdown point, the drone’s altitude, an chosen descent angle and distance to touchdown point. Once we have a glide path, we can find the error between the drone’s current altitude and the altitude of the glide path. We adjust the descent angle to reduce the error.
Formulas and Terms we may want to consider
Ideal glide path: = linear path determined by some predetermined descent angle
Ideal altitude: hideal(x) = hd + x * tan(η) where hideal() is the desired altitude, hd is our initial descent altitude and η is the predetermined descent angle at any point on x
Proportional Guidance Law:
...
where:
hd is initial descent altitude
h is the drone’s current altitude
η is the predetermined descent angle
ϵ is the parameter that controls how quickly the aircraft converges onto the ideal path
Given by
where
T is the touchdown point
P1 is projection of the drone directly below the ideal path
P2 a future point on the path shortly ahead of P1
Flare Maneuver (Landing) pg 621
Are we able to get data from Attitude Manager?
In order to properly conduct a flare maneuver right before touchdown, we need to ensure a constant, slightly positive pitch angle.
https://liu.diva-portal.org/smash/get/diva2:1055556/FULLTEXT01.pdf
| View file | ||
|---|---|---|
|
https://www.sciencedirect.com/science/article/pii/S2405896316315026
https://ieeexplore.ieee.org/document/6485292
Takeoff
The IEEE paper describes a similar strategy in takeoff and landing we have seen above by splitting each event into stages using waypoints. It is important to have access to speed data.
It discusses several strategies:
Throttle control
...
Tk represents the time constraints of the takeoff process. In our case, we can set some arbitrary value (probably). δT represents the throttle angle (engine power). Unlike previous strategies which simply used a constant engine power until takeoff speed is reached, this ensures smooth acceleration which might result in a more stable take off.
Determining pitch angle (do we control the flaps during takeoff?)
if so, look at IEEE paper
Climb stage
Proportional guidance can also be applied to the climb stage similar to the slope stage during landing.
We can create a reference climb angle which predetermined before launch. The formula is the same as the Slope stage calculations.
If we have access to altitude, speed and attitude data, we can adapt or rewrite the current takeoff and landing algorithm for a fixed wing drone using waypoints.
To Do:
read autonomous team’s algorithm