Bugfix for pathplanner > Circle Left/Right > End Condition > Pointing Towards Next

Description

static uint8_t conditionPointingTowardsNext()

The vector pointing towards next waypoint is being incorrecly constructed. Current waypoint position (in fact, this is the center of the circle) is used for start point and the next waypoint position as end point of the vector. This is incorrect, the start point should be the current aircraft position.

Demonstration of the issue:

Snapshot from log replay in GCS:

Airplane stopped following the circle when its heading was parallel to the line connecting WP9 & WP10. Few seconds later I understood somethings is wrong with autopilot and being many kilometers away decided to switch to manual for safety reasons.

 

The fix:

Get current aircraft position:

Use it for angle1 calculation:

 

Code:

 

I’ve built a firmware for Revolution FC with this fix and tested it with success. A pull request will follow.

What I’ve observed during tests:

If the radius of the circle is too small or the speed of the UAV too high AND the angular margin is set to a relatively small value, it is quite possible the UAV to overshoot (and miss) the right moment for waypoint transition. In this case, UAV just makes one more full circle and hopefully goes for the next wp after that.

Environment

None

Activity

Show:
Julian Lilov
6 days ago
Edited

One more issue addressed: after leaving circle, the pathDesired vector starting point is the center of the circle, i.e. previous waypoint.

This does not look pretty, the vector starting point should be the point where the UAV actually leaves the circle, heading to the next waypoint.

Here is the result of a brief test flight with the updatePathDesired() fix applied. Despite the windy conditions and the small radiuses and short leg lengths it proves the functionality of thе fix.

TheOtherCliff
6 days ago

Just some thoughts …

To fly an oval course, it looks like you need to set up a waypoint that flies to the edge of the semi-circle, and then set up e.g. a circle left that uses a different point, e.g. one that is straight off your left side.

The rest of this comment sounds like fixed wing path follower but it seems appropriate for copters in fast forward flight too.

Although it is functional, I can’t say that I am a real fan of the current fixed wing loiter functionality which is “constantly fly toward loiter point”.

Long ago, before Revo, when there was fixed wing pathfollower like code being written for CC3D, I made a change in my personal branch so that flying into a waypoint for loiter or next-waypoint, that it would determine left or right circle based on angle to next waypoint, and for a left circle it would (as I recall) use current speed and max bank angle to calculate the circle radius and something like 2R (a little less is needed but then there are servo and roll rate delays) out it would bank right (i.e. till tangent with the circle), then bank left to do the actual circle.

To me, this had the benefit of not requiring a lot of circle setup, and for loiter, being more deterministic than the current “constantly fly toward loiter point”.

Maybe I should stop now before I get into circling around a point or flying through a waypoint with the waypoint on the circle.

Julian Lilov
3 days ago

I agree this current implementation has some limitations, e.g. it's hard to code a path plan to make a full circle and then continue to next waypoint. The segment should not exceed 360-{angular margin of exit condition} degrees, when the exit condition is “Pointing Towards Next“.

Another complication: it does not really allow to have the next waypoint inside the circle (or even on the circumference) if the exit condition is “Pointing Towards Next“. Next waypoint should be outside the circle and at a reasonable distance.

My understanding is that current implementation had been mainly intended to allow to follow arc segments (entering and exiting the circular trajectory on tangents) between straight legs, thus making turns in smoothest possible way, for any kind of UAV. With latest fixes applied it seems to do this job well.

It would be a nice improvement in GCS, when drawing “Circle Left/Circle Right” mode waypoints, to display the arc that is not expected to be actually travelled, in faded color.

It may worth adding an entirely new “Loiter/Circle“ mode for waypoint, with mode param0 = desired radius (this makes most sense for planes) and code the pathfollower for fixed wing the way you described.

Assignee

Julian Lilov

Reporter

Julian Lilov

Labels

None

Components

Fix versions

Affects versions

Priority

Medium
Configure