Bug in pivot steering code

jeff567 · July 11, 2018, 12:12am

Hi, I’ve been following this forum and using Ardurover on boats for a while now. Been a quiet observer but it seems like I finally have a reason to post! I believe I found a bug in the steering code for pivot turns, and have a fix for it that works in simulation and that I’ll probably test out on a boat over the weekend.

I was tuning one of my boats to follow this series of waypoints:

I had set PIVOT_TURN_ANGLE to 135, so I was expecting to not get pivot turns on 90 degree turns. Oddly, I was getting a pivot on turn 3. My other 90 degree turn (turn 2), I was not getting a pivot. I guess turn 7 is just shy of 135, so no pivot there either.

In this youtube video, you can see the boat executing a pivot turn at turn 3 at 25 seconds

I was able to reproduce this problem in simulation. It seems that if you have a turn that is less than PIVOT_TURN_ANGLE, but the following turn is greater than PIVOT_TURN_ANGLE, you might erroneously get a pivot turn. In the case of the path above, the sharp turn at waypoint 4 was causing me to also pivot at waypoint 3.

The reason this happens is, while passing waypoint 3, ardupilot calculates what the final speed at waypoint 4 should be. Part of that process involves calling the rover.use_pivot_steering function, which can set pivot_steering_active to true, causing the rover to pivot at the current waypoint while calculating the final speed for the following waypoint.

Here’s the specific line of code that’s causing the issue

github.com

ArduPilot/ardupilot/blob/ba0fb3d9d2d514c0c6d96e3e21be3e0591c7e712/APMrover2/mode.cpp#L165


    _reached_destination = false;


    // set final desired speed
    _desired_speed_final = 0.0f;
    if (!is_equal(next_leg_bearing_cd, MODE_NEXT_HEADING_UNKNOWN)) {
        const float curr_leg_bearing_cd = get_bearing_cd(_origin, _destination);
        const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd);
        if (is_zero(turn_angle_cd)) {
            // if not turning can continue at full speed
            _desired_speed_final = _desired_speed;
        } else if (rover.use_pivot_steering(turn_angle_cd)) {
            // pivoting so we will stop
            _desired_speed_final = 0.0f;
        } else {
            // calculate maximum speed that keeps overshoot within bounds
            const float radius_m = fabsf(g.waypoint_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f));
            _desired_speed_final = MIN(_desired_speed, safe_sqrt(g.turn_max_g * GRAVITY_MSS * radius_m));
        }
    }
}

One way to fix this is to make a separate function for figuring out if the rover should pivot at the next waypoint. Here’s my solution:
https://github.com/ArduPilot/ardupilot/compare/master...jeff567:rover_pivot_turn_fix

This solves the problem in simulation. Happy to follow up with plots and logs. Plan on testing it out on the same path above over the weekend.

rmackay9 · July 11, 2018, 1:12am

Jeff,

Great find! That’s definitely a bug.

The solution looks good. You want to PR it? One small non-functional change is perhaps to collapse the stuff below, “// calc bearing error” into a single line… or a small number of lines… but in any case, nicely done!

jeff567 · July 11, 2018, 1:35am

sure! just submitted a PR.
thanks

rmackay9 · July 11, 2018, 12:03pm

… and merged… so we should probably push this out in a point release (i.e. Rover-3.4.2)… I might aim to do that next week sometime…

jeff567 · July 11, 2018, 12:29pm

great! looking forward to testing it out.

jeff567 · July 16, 2018, 2:29pm

I compiled and loaded firmware with this fix and things are working nicely now. seems to be obeying the PIVOT_TURN_ANGLE parameter of 135 degrees.

Here’s a video:

And bonus onboard video:

rmackay9 · July 17, 2018, 1:58am

Jeff,

Great, thanks for testing it and really love the videos. I stuck them on the ArduPilot.org facebook channel, hope that’s OK.

jeff567 · July 22, 2018, 5:27pm

something else I’ve been wondering… are rovers expected to stop when pivoting? I know while pivoting, the desired_speed_final gets set to zero, but it seems like in my boats and in simulations that they don’t necessarily have zero forward speed while pivoting.

I believe ideally, the rover will approach a waypoint, decelerate to zero velocity, execute a pivot turn (with no forward speed), then accelerate towards the next waypoint.

However looking at logs, it seems during a pivot turn, the desired speed doesn’t actually reach zero.

I haven’t dug into it much, but I think this may have to do with AR_AttitudeControl::get_desired_speed_accel_limited

rmackay9 · July 22, 2018, 11:43pm

Jeff,

It may not quite reach zero. I guess it’s possible to make it do that although that would also slow down the cornering. I think the key thing that people want is the vehicle to stick close to the path… so that should probably be the specific thing to look at and fix (if necessary).

jeff567 · July 22, 2018, 11:56pm

ok understood, thanks. just wanted to make sure I’m not trying to fix something that doesn’t need to be fixed!

lmy568 · July 29, 2019, 2:43am

hi, I think your boat’s performance very well when trigger the pivot , but in my boat always first turn to the reverse direction and then turn to the next waypoint , would you give me some advice ? very thanks