Values wrap around

I’m using a imu with the code from here Razor AHRS

The output for yaw is 0 at north, but I’ve tried to add an offset to that 0 is facing east-ish, where my pc screen is. This works quite well, but the offset goes past 360 degrees until original yaw position is past 0 degrees enough to cover the offset.

Is there a way to make the offset wrap back round to 0 once it hits 360?
I’m not asking for code, but maybe a pointer to where I should be looking. I’ve tried adding if statements, for when offset reaches 360, but it didn’t play nice.

Here’s what I’ve added to get the offset…

// Output angles: yaw, pitch, roll
void output_angles()
{
  if (output_format == OUTPUT__FORMAT_BINARY)
  {
    float ypr[3];  
    ypr[0] = TO_DEG(yaw);
    ypr[1] = TO_DEG(pitch);
    ypr[2] = TO_DEG(roll);
    Serial.write((byte*) ypr, 12);  // No new-line
  }
  else if (output_format == OUTPUT__FORMAT_TEXT)
  {
    while (offSetSorted != true) {
      offSet = (TO_DEG(yaw));
      offSetSorted = true;
    }
    Serial.print((TO_DEG(yaw)) - (offset)); Serial.print(",");
    Serial.print((TO_DEG(pitch)); Serial.print(",");
    Serial.println();
  }
}

I’m not very good at explaining things, so, hopefully people can understand what I’m getting at.

This is really confusing, and English is may native language. So you've got north at 0, east-ish, now what does that mean? Is it east or not east?

Try this:
What the imu reads What I want to see
N 0 ?
E ? ?
S ? ?
W ? ?

So what should the ? be, fill in the table. Compass rose goes one way, Cartesian coordinates the other, it's really not clear from your discussion or code.

Believe it or not, it's my native language too, it's been a long day.

Anyway, the small bit of code I've put above takes a reading from the imu, and then uses that as the offset. So if I point the imo east, then reset the arduino, the code takes the 1st reading from the imu yaw, and sets it as the offset(which would be 90 for east)

The code then reads the original yaw value, then takes away the offset value, and displays the offset-ed yaw value.

The problem is, if I move the imu clockwise to just past the real south(-179 value), the code then does real yaw minus offset....-179 - 90 = -269, when the offset-ed value should be around 90 degrees.

Edit: I've changed the above code to my simpler version(without mapping and what not)

I want to be able to point the imu in whatever direction i want, then reset, and the new offset value to be taken as north (0).
Basically without the offset, it works as it should. Ideal, if I din't mind rearranging the PC so that i face north to look at it. But with the offset added it does something like what is happening in this post..
gyro wrap

I've had a read, but can't get my head around it

So what you're looking for is a heading-up display?

For a simulator.
This is what I have working, kind of....

YouTube

In the video, I'm facing east, so there is a 90 offset added. The problem is, when I turn the imu clockwise to true South, that is when it craps out because it's the real mag reading of 180 + 90 degree offset. The offset doesn't wrap-around.

What imu?

It's a gy-85.

Acc, gyro and mag

Do a 360 for me and show me what readings you get for N, NE, E, SE, S, SW, W and NW. Some devices will give you 0-360, others +/- 180.

One way to deal with compass wrap is the following function, which you apply to the result of subtracting the yaw offset.

// routine to correct for compass wrap

int wrap360(int direction) {
	while (direction > 359) direction -= 360;
	while (direction <   0) direction += 360;
	return direction;
}

It's a +/-180

Direction    No offset   45°offset   90°offset   180°offset  -135°offset  -90°offset  -45°offset
N               0              45            90             180           -135             -90           -44
NE              45           90            135            225           -90              -45            0
E               90            135           180           270           -45              0               45
SE              135         180           225           315            0                45             90
S               180          225/-134    270/-90     360/0       45/-315       90/-270     135/-225
SW             -135         -91           -45           45              270            -225         -180
W              -90          -46            0               90             -225           -180         -135
NW             -45          0             45              135           -180            -135         -90

Hi jremington,

Would I have to map the original yaw output to 0 - 360?
Then apply the offset, then apply this code to outputted offset yaw value?

So the correction you want is

(IMU_reading + 360) % 360

Would I have to map the original yaw output to 0 - 360?

To use the code I posted, yes.

Most people use the 0-359 scale for yaw, because dealing with negative numbers is not so simple. The modulus operator ("%") doesn’t work the way you would like with negative numbers.

For example, if you wish to use the relation SW=-135, then the suggestion posted by DKWatson above doesn’t work. It returns 225 instead.

For navigation, I use the 0 to 359 (integer) scale for yaw, and for heading errors, the -180 to 180 scale because it is easier to map to steering corrections. For that, you need an additional routine:

// routine to calculate heading error in degrees, taking into account compass wrap

int heading_error(int bearing, int current_heading)
{
 int error = current_heading - bearing;
 if (error >  180) error -= 360;
 if (error < -180) error += 360;
 return error;
}

jremington:
For example, if you wish to use the relation SW=-135, then the suggestion posted by DKWatson above doesn't work. It returns 225 instead.

My understanding is that we're trying to convert the +/- into 0-360. In that way the compass heading can simply ve subtracted from 360 to give a heading up of zero which is needed for the display.

Finally got it working using DKWatsons code. I think this is how it is suppossed to be used!

  else if (output_format == OUTPUT__FORMAT_TEXT)
  {
    yawCom = (mapf(TO_DEG(yaw), 90, -90, 0, 359)); // map half of yaw to 0-360 degs
    i_heading = wrap360(yawCom);

// while to capture offset value. Need to have timer for readings to settle
      while (offSetSorted != true) {
      offSet = i_heading;
      offSetSorted = true;
    }

    yawOff = (i_heading - offSet) - 14; //create offset heading value, plus the final number is for tweaking
    imu_heading = wrap360(yawOff);
    final_heading = heading_error(0, imu_heading); // spilt 0-360 into -+180

    Serial.print(final_heading); Serial.print(", "); //wrap360
    Serial.print(TO_DEG(pitch)); Serial.print(",");
    Serial.println();
  }
}
// routine to calculate heading error in degrees, taking into account compass wrap
float heading_error(float bearing, float current_heading)
{
  float error = current_heading - bearing;
  if (error >  180) error -= 360;
  if (error < -180) error += 360;
  return error;
}
// routine to correct for compass wrap
float wrap360(float direction) {
  while (direction > 359) direction -= 360;
  while (direction <   0) direction += 360;
  return direction;
}

What is the following supposed to be doing? Why "half of yaw"?

   yawCom = (mapf(TO_DEG(yaw), 90, -90, 0, 359)); // map half of yaw to 0-360 degs

You should be taking the values directly from the IMU, the "no offset" column of the table of reply #10, add or subtract the offset, then call wrap360() or use the formula in reply #12. All done.

What is the purpose of the following line?

    final_heading = heading_error(0, imu_heading); // spilt 0-360 into -+180

I'm using it for flightgear flight sim using it's protocol setting.
Half yaw means for a 90 degree look to the right, I only have to move my head 45 degrees, that way I can still see the screen.
Then the final heading bit converts it all back to +-value what the software uses.
I know there probably is an easier way to do all this, but after a day at work, then home to spend time with the family, I don't really have the mental capacity to think that hard ha ha