Plane: Smooth Consecutive Turns Support

[jrbronkar]

Feature request

Background

I have already worked on the merged #25873 to unblock this work - I will be discussing from a developer perspective. This issue focuses mainly on not really a bug, but a feature gap of LOITER_TURNs in ArduPlane, namely smooth consecutive turns. I am willing to tackle most/some of the work - this issue is to flesh out and consolidate the discussions we have had during last few Dev Calls.

Summary of Desired Feature

I want to fly (using a normal ArduPilot Mission/Mission Commands) paths with consecutive curves.

Let's take something like this relatively simple curve (flying left to right) that is kinematically possible to fly on a normal Plane and which consists of a straight (starting at blue) into a quarter circle, then into another quarter circle, then into a straight (exit at green). I believe this is impossible using ArduPilot Mission Commands (LOITER_TURNS, Waypoints) in a Mission on ArduPlane as it stands today - and in fact all precise consecutive turns are impossible, see "Proposed Developer Diagnosis of Issue".

Impossibility Explanation (using Primitives)

To fly this path, you might try to upload something like the below with ArduPilot Primitives.

Flying this path from left->right:

1. Waypoint to specify entrance to turn
2. Clockwise LOITER_TURN with desired radius + center to represent first quarter turn.
3. Waypoint to indicate exit of first LOITER_TURN (denoted by middle hollow point)
4. CounterClockwise LOITER_TURN with desired radius + center to represent second quarter turn.
5. Final waypoint somewhere along the green segment so you know when to exit the second LOITER_TURN.

But it doesn't seem to work - why? (note: above proposed path maps to waypoints 3-7)

1. We reach Waypoint 3
2. Switch onto first LOITER_TURN and ArduPilot checks exit criteria
   a. Exit criteria is verify_loiter_heading for both branches of this function! of the verify_loiter_turns function (Note: this means LOITER_TURNS has heading required in all cases!)
   b. verify_loiter_heading calls isHeadingLinedUp with the next location
   c. isHeadingLinedUp hits logic that breaks out of the loiter immediately - since the "next" waypoint, or Item 5 is within the loiter radius + 5% (it is on the loiter radius)
3. Now that we have broken out of the first LOITER_TURN we transit normally to Item 5, but we are nearly perpendicular to the desired second loiter.
4. We reach item 5, and we switch quickly onto the second LOITER_TURN and we check exit criteria - isHeadingLinedUp again - and we point towards the next and final waypoint (item 7) pretty immediately and exit the second LOITER_TURN

Great. Shouldn’t place waypoints after a loiter within the loiter radius + 5%, got it. What if I just remove that waypoint then?

Doesn't work either! we

1. Reach Waypoint 3, switch onto first LOITER_TURN
2. Stay on first LOITER_TURN for a very small time after waypoint 3 until isHeadingLinedUp with the center(!) of the second LOITER_TURN.
3. Switch to the second LOITER_TURN. As we transit to the second LOITER_TURN, we reach isHeadingLinedUp with waypoint 6 and exit the second LOITER_TURN loop (even though we haven't reached the loiter yet) and start transiting to waypoint 6.

Proposed Developer Diagnosis of Issue

isHeadingLinedUp is required in all cases for a LOITER_TURN, and is relatively inflexible - it does not allow for waypoints to be on the radius of the circle + 5%, that one otherwise might be able to use as exit criteria for a discrete turn.

Even if you operate under the world of fractional turns that recently merged, you wouldn't be able to chain two quarter LOITER_TURNS together to implement the desired smooth consecutive turn path, because the isHeadingLinedUp check is required in all cases. you would finish a quarter of the turn, and then try to turn to get heading with the center of the next turn, then go back to the second turn, then get heading with the next point, instead of: finish quarter of the first turn, switch to second turn cleanly, finish quarter of second turn, continue on path. I can demonstrate this in the below comments if anybody would like to see in SITL.

Describe the solution you'd like
I would propose that both elements of this should be fixed, though I would note either should resolve the issue, albeit at different levels:

isHeadingLinedUp is required in all cases for a LOITER_TURN
isHeadingLinedUp is relatively inflexible

1. Implement Heading Required field for ArduPlane LOITER_TURNs to align with the mavlink spec

• If we implemented the Heading Required field, you would be able to use fractional turns + heading required disabled/enabled checks to correctly chain together turns.

2. Make the isHeadingLinedUp more flexible. See Plane: Patch heading check for turn flexibility #25938 as a proposed solution to more dynamically configure the exit criteria based where that "next" waypoint is. Currently the heading tolerance used for the exit criteria is hard coded.

   ardupilot/ArduPlane/mode_loiter.cpp

   Lines 61 to 82 in 8c72304

   	// Return true if current heading is aligned to bearing_cd.
   	// Tolerance is initially 10 degrees and grows at 10 degrees for each loiter circle completed.
   	// get current heading.
   	const int32_t heading_cd = (wrap_360(degrees(ahrs.groundspeed_vector().angle())))*100;
   	const int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd);
   	/*
   	Check to see if the the plane is heading toward the land
   	waypoint. We use 20 degrees (+/-10 deg) of margin so that
   	we can handle 200 degrees/second of yaw.
   	After every full circle, extend acceptance criteria to ensure
   	aircraft will not loop forever in case high winds are forcing
   	it beyond 200 deg/sec when passing the desired exit course
   	*/
   	// Use integer division to get discrete steps
   	const int32_t expanded_acceptance = 1000 * (labs(plane.loiter.sum_cd) / 36000);
   	if (labs(heading_err_cd) <= 1000 + expanded_acceptance) {

• If you implemented this, you could fly the first example cleanly without running into breakout logic.
• See PR Desc for a nice SITL demonstration!:
  

Describe alternatives you've considered
See description of two options above, I consider both of these to be valid approaches that should both land on master. I would think landing the already implemented patch PR before the likely more intensive "implement heading required" makes sense.

Edited after Lua Script comment: I believe this is a limitation or feature gap of our Mission Commands primitives that can be improved and provide much more flexibility/intuitiveness to the average user instead of having to script something. I believe flying a smooth, flat curve into another curve shouldn't need to fall in the acrobatics realm and can be achieved with the existing vanilla primitives.

Phew. That was long. I have given this a deal of thought, and I would appreciate any thoughts, points of discussion, or other ideas for solutions.
Platform
[ ] All
[ ] AntennaTracker
[ ] Copter
[X] Plane
[ ] Rover
[ ] Submarine

Additional context
Add any other context or screenshots about the feature request here.

[timtuxworth]

Here's another option - implement it in Lua by basically writing this as an "aerobatic trick". Checkout the "Scripted Aerobatics" examples https://ardupilot.org/plane/docs/common-scripting-aerobatics.html.

[IamPete1]

Do you actually want to program such moves with loiter turns, or if we were to do it automatically would what be better? It would be a big re-work, but we could update to use Dubins paths for FW navigation.

[jrbronkar]

I'd like to program such moves with loiter turns. I am willing to work on it as I did for the fractional loiter turn support, and the changes should be similar net benefits to Missions by expanding the use cases of loiter turns to either A) align more fully with the mavlink spec, B) be more flexible and less hardcoded (heading check patch), or C) both (A) and (B).

My desire for this issue was detail out this problem I have been noticing in SITL and see if my proposed fixes seem sane and reasonable to the problem (and would be approved). perhaps we can chat at the next dev call?

[IamPete1]

One problem I found while trying this out is that because we scale the loiter radius with altitude its very hard to manually program such missions if your above sea level because the GCS's shown radius's are not the what we will fly.

This also highlights a bug in the original break out code because it assumes we will fly at the un-corrected radius. I wonder if this is why we needed those fudge factors and increasing error bound in the first place....

In any case I have done a alternate fix here: https://github.com/IamPete1/ardupilot/tree/loiter_heading_linedup

[jrbronkar]

gotcha. appreciate that flag. I will take a look at your branch, thank you!

[jrbronkar]

This PR appears to resolve this issue (represents the branch linked above that Pete suggested). tagging for visibility into the smooth turn issues this was geared, in part, at addressing.

[magicrub]

For what it's worth, I disable that altitude radius thing because we've implemented it wrong. Its meant to keep us in our flight envelope but it should be enforcing a minimum, not always scaling. Not being able to actually control your aircraft path is kind of a party foul.

[IamPete1]

For what it's worth, I disable that altitude radius thing because we've implemented it wrong. Its meant to keep us in our flight envelope but it should be enforcing a minimum, not always scaling. Not being able to actually control your aircraft path is kind of a party foul.

I agree.

[Ryanf55]

I'm super interested this, or a more generic capability for a trajectory-following controller following 3D dubins curves.

This could be used in #2380 , and allow a companion computer to generate more intelligent paths than what was shown possible here: #25722

Would you all consider just implementing a trajectory-following controller? I'm happy to help and start in 2D to make it more feasible.

[IamPete1]

Would you all consider just implementing a trajectory-following controller? I'm happy to help and start in 2D to make it more feasible.

We can use the L1 controller for following arbitrary paths, its in the original paper, but we have only implemented straight lines and circles. So there are two steps, the first is to add support for arbitrary paths to the L1 and then the second is to define that path. Once that is done this whole logic problem were trying to fix in #26102 is gone. We don't need to decide when to switch from circle "mode" to straight line "mode" in L1, we just give it the full path.

[jrbronkar]

Chatted at the most recent dev call - going to put some notes here for reference:

Sounds like there is some excitement about potential future dubins' planning/path support for ArduPlane but it also sounds like we can move ahead with #26102 to unblock this issue in the absence of more complex fixes short-term as it is a net benefit in improving use/usage of the current primitives.

[jrbronkar]

closing. after extensive testing: #26102 has resolved the issue.