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

Status

Assignee

Julian Lilov

Reporter

Julian Lilov

Labels

None

Components

Fix versions

Affects versions

Priority

Medium
Configure