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:

1 2 PositionStateData positionState; PositionStateGet(&positionState); // current aircraft position

Use it for angle1 calculation:

1 float angle1 = atan2f((nextWaypoint.Position.North - positionState.North), (nextWaypoint.Position.East - positionState.East));

 

Code:

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 static uint8_t conditionPointingTowardsNext() { uint16_t nextWaypointId = waypointActive.Index + 1; if (nextWaypointId >= UAVObjGetNumInstances(WaypointHandle())) { nextWaypointId = 0; } WaypointData nextWaypoint; WaypointInstGet(nextWaypointId, &nextWaypoint); PositionStateData positionState; PositionStateGet(&positionState); // current aircraft position // float angle1 = atan2f((nextWaypoint.Position.North - waypoint.Position.North), (nextWaypoint.Position.East - waypoint.Position.East)); // wrong starting point! float angle1 = atan2f((nextWaypoint.Position.North - positionState.North), (nextWaypoint.Position.East - positionState.East)); VelocityStateData velocity; VelocityStateGet(&velocity); float angle2 = atan2f(velocity.North, velocity.East); // calculate the absolute angular difference angle1 = fabsf(RAD2DEG(angle1 - angle2)); while (angle1 > 360) { angle1 -= 360; } if (angle1 <= pathAction.ConditionParameters[0]) { return true; } return false; }

 

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

REL-16.09 - Black Rhino

Priority

Medium
Configure