compass heading changes on tilt of device.

Hi there,

Im very new to arduino. Im working on a project that a little simular to a reverse geocache. but with a twist. I want to give the user a box with a display with an arrow on it that will show the heading of the next waypoint. I want to use an 3-axis magnetometer for compass and a gps module to get there current position so i i can calculate the next arrow position.

The next waypoint will be loaded in using a rfid reader and tag.

I got the gy87 clone working.

The problem i'm encountering now is that when i tilt the magnetometer, the heading on my serial monitor isnt pointing to north anymore.

The code i'm using for this part of the project is written by Dthain and found on the next link. https://github.com/dthain/QMC5883L

Is it possible to disable the Z axis so the heading doesnt change anymore when i tilt the device?

You need to build a tilt-compensated compass (MAG+ACC), otherwise this will not work.

+1 for tilt-compensated compass. Pololu has some very simple vector based code for their magnetometer/accelerometer sensors, like the LM303D.

Alternatively, you could add a round bubble level to the project.

I'm encountering the same on my mpu-9250.

You will need to run a quaternion IMU in order to compensate bank and pitch. How to get heading from that is just what i am trying to figure out

Can you tell what hardware youre using?

@Miniflyer

 MahonyQuaternionUpdate ( get_aX(), get_aY(), get_aZ(), get_gX(), get_gY(), get_gZ(), get_mX(), get_mY(), get_mZ() );
        // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
        // In this coordinate system, the positive z-axis is down toward Earth.
        // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
        // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
        // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
        // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
        // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
        // applied in the correct order which for this configuration is yaw, pitch, and then roll.
        // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
        yaw = atan2f(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
        pitch = -asinf(2.0f * (q[1] * q[3] - q[0] * q[2]));
        roll = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
        pitch = ( (pitch * 180.0f) / PI);
        yaw  *= 180.0f / PI;
        yaw   -= 14.0f; // Declination
        roll = ( (roll * 180.0f) / PI);

Gives a good Great Circle from there you'll need to extract the off-set for a reading that can be used in error correcting.

All you need for a tilt compensated compass is an accelerometer and a magnetometer. Theory here, which also includes the absolutely essential calibration step. But Pololu's vector based approach is much, much simpler.

OK, i just did some copy and pasting, but i’m not getting the MahoneyQuaternionUpdate correctly.

What am i doing in the code? I’m reading the ax, ay, az, gx, gy, gz, hx, hy and hz from the 9250 via I2C and placing them in the mahoney order. Further down i am running the mahoney update.
Somewhere on the handover i’m not getting the current values, as my output on the angles is always 0,0, 13.13.

I’m not sure where the handover of data gets lost, but it does…

AHRS_MPU9250.ino (6.84 KB)

Ok,ive spent a few hours today trying to figure out how the handover works. Not even changing designators to the "read from sensor" values seems to work. Not really getting anywhere....any hints?

Looks pretty simple to me. From your .ino code...

IMU.readSensor();

Reads the current sensor values into the object named IMU.

  _ax = (float) IMU.getAccelX_mss();

Converts the X-axis acceleration to meters per second per second and saves that value in your local variable called _ax.

...do the same for all 8 other axes...

  delay(20);

Well, you aren't going to use delay in your final project but it's OK to use it for this simple demonstration of the sensor making some numbers.

  MahonyQuaternionUpdate ( _ax, _ay, _az, _gx, _gy, _gz, _hx * 10.0f, _hy * 10.0f, _hz * 10.0f );

This updates the global array q[] with the filtered quaternion.

Note that the Mahoney filter requires a "deltaT" which is the time difference between samples. You've defined deltaT as zero when you're actually taking samples slightly more than 20ms apart.

Yea, I got a few hints.

your deltat, how are you handling that?

These variables must be handled:

float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};           // vector to hold quaternion
float eInt[3] = {0.0f, 0.0f, 0.0f};              // vector to hold integral error for Mahony method
float deltat = 0.0f;                             // integration interval for both filter schemes
// #define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
// const float Ki = 1.7f;
// const float Kp = 7.50f;
const float Ki = .1f;
const float Kp = 1.0f;

Use these for a MPU 9250:

// const float Ki = 1.7f;
// const float Kp = 7.50f;

The values I am using are for a LSM9DS1.

deltat should be as precise as possible. I am using the 64bit uS counter in the ESP32.

 float TimePast = get_ESP32_uS();

float TimeNow = get_ESP32_uS();[/code] I declare and set two variables.

Here I generate the deltat

 TimeNow = get_ESP32_uS();
      deltat = ( TimeNow - TimePast);

I, highly, recommend you do not use 0.0f for the Ki or Kp. Ki, Kp is used to fill the eInt[3], which will be the error correction used by the filter.

You will need to convert your magnometer readings to mT from uT by multiplying by 10, as shown in the above post.

Thanks for the corrected values.

Ahhhhh, yes of course. Integrating over a timeperiod of 0 will of course always output 0 as a result.

Ok, back to work, i will try to study your code to see where you are taking the readings, and also how to take time readings on the arduino in a useful manner. I will let you know how it goes :)

Thanks

Idahowalker: deltat should be as precise as possible. I am using the 64bit uS counter in the ESP32.

So for highest precision i should consider using micros instead of millis?

Yes. An error of 1ms in 20 is 5%.

But I don't think it will make 5% difference to the computed result, unless you are moving fast (i.e. vibration.)

Miniflyer: So for highest precision i should consider using micros instead of millis?

Also, you may find that the calculations are done, as mine are, in 290uS. A deltat based upon mS would 'see' 290uS as 0mS. The 0 delta will result, in the Mahony method producing a 0 output.

OK, a bit further…current code attached.

What have i changed?
@Idahowalker: 1.7 and 7.50 have been used all along. Also the “*10.0f” is included in the Update.

-deltat taken into consideration using 2 time samples in microns()
-delay 20 taken out of the sampling

I take the starting time at the beginning of the loop and the stopping time at the end of the sampling. Ive also tried inserting it after the mahoney update and at the end of the loop.

Good news: it is now outputting data at output numbers in the range where they should be (ranging +/-300)

Bad news: they are drifting randomly and rather slow (about 1deg/s). I can slightly influence direction and speed of drift by moving sensor, but it is not reading actual anything…

I suspect i am either using wrong units, or taking the time samples incorrectly (or a combination).

AHRS_MPU9250.ino (6.93 KB)

At this point you may want to take the roll, pitch, yaw and send it through a simple Kalman Filter, like this one #include <SimpleKalmanFilter.h>, which you can get from here GitHub - denyssene/SimpleKalmanFilter: A basic implementation of Kalman Filter for single variable models.. You’ll use deltat for the q: Process Variance, which should be updated before each update.

I thought the calman filter was just to smooth out jittery results? Right now they are not jittery, they are not responding to my movement and drifting randomly. Tried some other start and end points for the delta t peasurement. All not working....

Miniflyer: I thought the calman filter was just to smooth out jittery results? Right now they are not jittery, they are not responding to my movement and drifting randomly. Tried some other start and end points for the delta t peasurement. All not working....

From the description of the problem you are having, I'd say the issue is on line 2356 of your code.

I put in code that allows me to read the raw values from the IMU on the screen, when I am troubleshooting:

void fGetIMU( void *pvParameters )
{
  if ( fInitializeDevice( ) )
  {
    log_i( " SPI device init " );
    if ( fInitializeAG() )
    {
      log_i( "AG init" );
      if ( fDO_AG_ID() )
      {
        log_i( " AG self ID'd " );
        if ( fInitializeM() )
        {
          log_i( "Init Magnetometer" );
          if ( fDO_M_ID() )
          {
            log_i( " M self ID'd: &d", getMAG_ID_OK() );
            fReboot();
            vTaskDelay( 50 );
            if ( fEnableGandA() == false ) // enable gyros and accelerometers
            {
              log_i( " Fail: Enable Gyro and Accelerometer " );
            }
            if ( fEnableM() == false ) // enable gyros and accelerometers
            {
              Serial.println( " Fail: Enable Magnetometer" );
            }
            if ( setupAccelScale( LSM9DS1_ACCELRANGE_8G ) )
            {
              log_i( " Fail: SetupAccelScale" );
            }
            if ( setupGyroScale ( LSM9DS1_GYROSCALE_2000DPS ) )
            {
              Serial.println( " Fail: setupGyroScale" );
            }
            if (setupMagScale( LSM9DS1_MAGGAIN_12GAUSS ) )
            {
              Serial.println( "FailL setupMagScale" );
            }
          } // if ( fDO_M_ID )
          else
          {
            log_i( "fDO_M_ID, fail: %d", returnHighBits() ); //each LSMDS1 may have its own ID
          }
        } // if ( fInitializeM() )
      } // if ( fDO_AG_ID() )
      else
      {
        log_i( "fDO_AG_ID, fail: %d", returnHighBits() );
      }
    } // if ( fInitializeAG() )
  } // if ( fInitializeDevice( ) )

  boolean DidBias = false;
  // boolean DidBias = true; // for testing
  float TimePast = get_ESP32_uS();
  float TimeNow = get_ESP32_uS();
  TickType_t xLastWakeTime = xTaskGetTickCount();
  TickType_t xFrequency = 25;
   for (;;)
  {
    vTaskDelayUntil( &xLastWakeTime, xFrequency );

    // read the sensor
    if ( DidBias )
    {
      TimeNow = get_ESP32_uS();
      deltat = ( TimeNow - TimePast);
      if (  getLSM9DS1_ID_OK() &&  getMAG_ID_OK() ) // then do things
      {
        fReadAccelerometers();
        fReadGyros();
        fReadMagnetometer();
        // fPrintIMU();
        MahonyQuaternionUpdate ( get_aX(), get_aY(), get_aZ(), get_gX(), get_gY(), get_gZ(), get_mX(), get_mY(), get_mZ() );
        yaw = atan2f(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
        pitch = -asinf(2.0f * (q[1] * q[3] - q[0] * q[2]));
        roll = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
        pitch = ( (pitch * 180.0f) / PI);
        yaw  *= 180.0f / PI;
        yaw   -= 14.0f; // Declination
        roll = ( (roll * 180.0f) / PI);
        // Serial.print( roll );
        // Serial.print( ", " );
        // Serial.print( pitch );
        // Serial.print( ", " );
        // Serial.print( yaw );
        // Serial.print( ", " );
        // Serial.println( );
        if ( xSemaphoreTake( sema_X, xZeroTicksToWait ) == pdTRUE ) // grab semaphore, do not wait
        {
          xQueueOverwrite( xQ_X_INFO, (void *) &roll );
          xEventGroupSetBits( eg, evtTweakServoX );
        }
        if ( xSemaphoreTake( sema_Y, xZeroTicksToWait ) == pdTRUE ) // grab semaphore, do not wait
        {
          xQueueOverwrite( xQ_Y_INFO, (void *) &pitch );
          xEventGroupSetBits( eg, evtTweakServoY );
        }
        TimePast = TimeNow;
      } // if ( LSM9DS1_ID_OK && M_ID_OK ) // then do things
      else
      {
        Serial.print ( " LSM9DS1_ID_NOT_OK ");
        Serial.print ( getLSM9DS1_ID_OK() );
        Serial.print ( " or MAG_ID_OK not OK " );
        Serial.println(  getMAG_ID_OK() );
      }
    } // if ( DidBias )
    else
    {
      calibrate(); // accelerometer and gyro
      // log_i( " aXbias = %f, aYbias = %f, aZbias = %f", get_aXbias(), get_aYbias(), get_aZbias() );
      // log_i( " gXbias = %f, gYbias = %f, gZbias = %f", get_gXbias(), get_gYbias(), get_gZbias() );
      fCalculateM_offset( );
      //      Serial.print( "calibration values ");
      //      Serial.print( ", ax ");
      //      Serial.print ( mpu9250.getAXbias(), 6 );
      //      Serial.print( ", ay ");
      //      Serial.print ( mpu9250.getAYbias(), 6 );
      //      Serial.print( ", az ");
      //      Serial.print ( mpu9250.getAZbias(), 6 );
      //      Serial.print( ", gx ");
      //      Serial.print ( mpu9250.getGXbias(), 6 );
      //      Serial.print( ", gy ");
      //      Serial.print ( mpu9250.getGYbias(), 6 );
      //      Serial.print( ", gz ");
      //      Serial.print ( mpu9250.getGZbias(), 6 );
      //      Serial.print( ", mx ");
      //      Serial.print ( mpu9250.getMXbias(), 6 );
      //      Serial.print( ", my ");
      //      Serial.print ( mpu9250.getMYbias(), 6 );
      //      Serial.print( ", mz ");
      //      Serial.print ( mpu9250.getMZbias(), 6 );
      //      Serial.println();
      DidBias = true;
      ////        /* setting the interrupt */
      xEventGroupSetBits( eg, evtLIDAR_ServoAspectChange ); // start the LIDAR scanning
    }
    //    Serial.print( "fGetIMU " );
    //    Serial.print(uxTaskGetStackHighWaterMark( NULL ));
    //    Serial.println();
    //    Serial.flush();
    xLastWakeTime = xTaskGetTickCount();;
  } //  for (;;)
  vTaskDelete( NULL );
} // void fGetIMU( void *pvParameters )

If the input values are not changing the output values will not...

Anyways, consider your descriptions of your issues and how would the descriptions help someone else help you.

Idahowalker: From the description of the problem you are having, I'd say the issue is on line 2356 of your code.

I doubt that.....it is flawless and clean. But scroll up a bit, you will see that i too have added prints of the raw data, they are just commented out on this status of code....they sense fine it seems.

Idahowalker: Anyways, consider your descriptions of your issues and how would the descriptions help someone else help you.

I posted full codes since development starts, i posted problem description, changes made, things i tried and a description of results i am getting. ....

OK i have taken sample readings after getting noplace with the coding. The error must be in the integration, or in the quaternion code itself. Convention:

Line 1: Accelleration x;y;z; Gyro x;y;z; Magnetometer x;y;z; Line 2: q0, q1, q2, q3 Line 3: pitch, roll, yaw.

What i see: Accelx and y show slight readings where they should be near 0. They are not far off though. The Z value is with 9.3 roughly in the ballpark. Gyros are all near 0, which is correct also Magnetometer values are also keeping roughly constant

The quaternion dates i cannot interpret, but pitch roll and yaw are nowhere near where they should be with these raw inputs......

1.087023;0.962518;-9.337862;-0.000272;-0.000341;0.000172;13.573575;-26.966167;19.024675; 0.00;0.86;-0.45;-0.25 25.61;165.40;-45.68

1.091812;0.943364;-9.347440;-0.000804;-0.000341;0.000172;13.573575;-26.966167;19.024675; -0.00;-0.98;0.19;0.03 3.06;179.41;-8.52

1.082234;0.972095;-9.352229;0.000794;-0.000341;-0.000361;12.125727;-26.604206;19.373752; 0.00;0.69;-0.57;0.44 -37.55;-140.62;-79.82

1.072657;0.957729;-9.361806;-0.000272;0.000192;0.000172;12.125727;-26.604206;19.373752; -0.00;-0.17;0.91;0.37 7.37;137.01;-148.28

1.082234;0.957729;-9.342652;0.000261;0.000192;0.000704;13.030632;-27.147150;19.897367; 0.00;0.72;-0.59;-0.36 31.27;150.41;-73.54

1.091812;0.972095;-9.357017;0.000261;0.000192;0.000704;13.030632;-27.147150;19.897367; -0.00;-0.99;0.07;0.08 8.69;179.35;4.58

1.087023;0.962518;-9.342652;-0.000804;0.000724;-0.000361;13.935537;-26.604206;18.675600; 0.00;0.68;-0.20;0.71 -73.69;-87.86;-95.22