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.

Answer 2

As a hack, have you tried sending it on a 720° heading?

Otherwise, I'd have to see more details of the properties and methods you are calling to make any further sense.

Answer 3

This might sound overly simple, but cant you just use the delta between the target vector and the current vector so that your always measuring the distance from 0 and your bearing indications now wrap at -180 and 180?

I'm not sure how you're calculating the bearing either, but a quick look at the Ardupilot shows the bearing calculation as:

int calc_bearing(float flat1, float flon1, float flat2, float flon2)
{
  float calc;
  float calc2;
  float bear_calc;
  float diflon;
  //I've to spplit all the calculation in several steps. If i try it to do it in a single line the arduino will explode.
  flat1=radians(flat1);
  flat2=radians(flat2);

  diflon=radians((flon2)-(flon1));

  calc=sin(diflon)*cos(flat2);
  calc2=cos(flat1)*sin(flat2)-sin(flat1)*cos(flat2)*cos(diflon);

  calc=atan2(calc,calc2);

  bear_calc= degrees(calc);

  if(bear_calc<=1){
    bear_calc=360+bear_calc;
  }
  return bear_calc;
}

and correcting for heading error by clamping:

int heading_error(int PID_set_Point, int PID_current_Point)
{
 float PID_error = PID_set_Point - PID_current_Point;

 if (PID_error > 180) {
   PID_error -= 360;
 }

 if (PID_error < -180) {
   PID_error += 360;
 }

 return PID_error;
}