Adjusting for compass wrap around in a navigation application
- by chris12892
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!
