Adjusting for compass wrap around in a navigation application

Question

I have a application where I am guiding a vehicle on compass headings and I am having an issue when the vehicle is crossing from 360 degrees to 0 degrees.

In this case, there are no smarts in the control loop to compute that the nearest way to turn to follow a heading.

For example, if the vehicle is instructed to follow a heading of 360 degrees, it will inevitably drifts a few degrees to ether side. If it drifts over to 0+ degrees, the control loop will go nuts and try to steer the vehicle all the way around to get it to 360 degrees again.

Is there a graceful way to deal with this?

The way the navigate function is written, I use an external PID controller class and I calculate the heading like this:

            lock (steering)
        {
            if (!Engaged)
            {
                return;
            }

            double mv = 90 + Trim + pidController.CalculateCorrection(flyHeading, currentHeading);

            steering.Degree = mv;
        }

Thanks!

Answer 1

Assuming you can't change the CalculateCorrection method, add these lines before the call to ensure that the delta is in a reasonable range.

if (flyHeading - currentHeading > 180) currentHeading+=360;
else if (flyHeading - currentHeading< -180) currentHeading-=360;

If you can fix CalculateCorrection, put the clamp there, like in @Greg Buehler's example.