Fix velocities and 3D in pathplanner pathfollower

Description

path planner/follower velocities are not implemented correctly, for example in paths.c, if path_endpoint is given a zero ending velocity it looks like it will never even start. path_vector fails if starting (or ending?) velocity is zero.

flying a circle could/should calculate a bank angle based on current velocity and desired circle radius and use that as a neutral rather than zero bank and always being wrong and outside the desired circle.

Is FlightModeSettingsPositionHoldOffset really needed? When? That really feels like a hack of an attempt to avoid some issue that should be handled correctly.

Last time I tried, RTB didn't work well. Short leg. Very slow. A user says that the only thing that makes RTB velocity faster is PosP, but that has other bad effects. Also that RTB takes way too long at base before landing.
https://forum.librepilot.org/index.php?topic=471.msg3466#msg3466

Fly endpoint should do something with max bank angle to determine the max acceleration and use the max acceleration to ramp up/down the speed. Probably try to use the highest velocity for the longest time whether going from fast to slow or slow to fast. Realize that the leg may be 1km or 1m. In the 1km case fly fast most of the way. In the 1m case realize that you won't be able to get up to full speed so don't even try.

What if there is significant wind such that the max bank angle can barely make headway? The acceleration calculations fail and you won't reach full speed going upwind, and when braking going downwind, you will overshoot the endpoint.

RTB velocities are set to zero in plans.c. They should be a percentage of max velocity. Maybe we want a new setting called RTBVelocityPercent (whatever) that defaults to say 70% (of maxh velocity or even vector sum of maxh and maxv). Make sure that you don't exceed either maxh velocity or maxv velocity.

Do 3D paths work correctly? They did at one time. You should be able to use any of the modes in any valid 3D way and the aircraft should stay on the path, even in wind.

Related to

Environment

None

Assignee

Unassigned

Reporter

TheOtherCliff

Components

Fix versions

Affects versions

Priority

Medium
Configure