search

kriswiner / MPU9250

Arduino sketches for MPU9250 9DoF with AHRS sensor fusion
1.03k stars 471 forks source link

Arduino UNO with MPU9250 Yaw Reading #193

Open scheng98 opened 6 years ago

scheng98 commented 6 years ago

Hi Kris,

Thanks a lot for your nice coding work on MPU9250 IMU! I got your latest MPU9250BasicAHRS.ino code from GIT and ran it on my Arduino UNO with MPUT9250.

I made two small changes,

  1. serial board rate changed from 38400bps to 115200bps. the msg format was changed a little bit as well.
  2. comment out all LCD codes as I don't have them at all,

I got it working fine when rotating it 360 degrees anti-clockwise or clockwise. No Yaw drifting was found. However, I found an issue when rotating it 90 degree anti-clockwise, and repeat it 3 more times.

---- original position rate = 228.31 Hz ax = 2.99 ay = -74.89 az = -1076.97 mg gx = 0.04 gy = 0.12 gz = 0.16 deg/s mx = -6 my = -222 mz = -384 mG q0 = 0.00 qx = -0.04 qy = -1.00 qz = 0.03 <qW:0.0000,qX:0.0000,qY:0.0000,qZ:0.0000,eX:161.9970,eY:-0.0179,eZ:-176.1991>

---- 90 Anti-clockwise rotate rate = 228.41 Hz ax = -4.94 ay = -75.56 az = -1083.07 mg gx = 0.05 gy = 0.15 gz = 0.14 deg/s mx = -281 my = -132 mz = -433 mG q0 = -0.02 qx = 0.51 qy = -0.86 qz = 0.03 <qW:0.0000,qX:0.0000,qY:0.0000,qZ:0.0000,eX:-131.9031,eY:0.1269,eZ:-175.9421>

---- 90 Anti-clockwise rotate rate = 228.47 Hz ax = -6.04 ay = -78.55 az = -1079.22 mg gx = 0.03 gy = 0.11 gz = 0.08 deg/s mx = -185 my = 180 mz = -438 mG q0 = -0.03 qx = 0.94 qy = -0.34 qz = 0.01 <qW:0.0000,qX:0.0000,qY:0.0000,qZ:0.0000,eX:-53.7203,eY:0.2318,eZ:-175.8613>

---- 90 Anti-clockwise rotate rate = 228.81 Hz ax = -0.12 ay = -77.27 az = -1074.28 mg gx = 0.03 gy = 0.05 gz = 0.09 deg/s mx = 108 my = 44 mz = -402 mG q0 = -0.03 qx = 0.82 qy = 0.57 qz = -0.02 <qW:0.0000,qX:0.0000,qY:0.0000,qZ:0.0000,eX:55.0744,eY:0.4858,eZ:-175.9031>

---- 90 Anti-clockwise rotate rate = 227.37 Hz ax = 0.12 ay = -77.51 az = -1076.90 mg gx = 0.10 gy = 0.08 gz = 0.21 deg/s mx = -16 my = -236 mz = -380 mG q0 = -0.00 qx = 0.03 qy = 1.00 qz = -0.04 <qW:0.0000,qX:0.0000,qY:0.0000,qZ:0.0000,eX:162.8931,eY:0.0246,eZ:-175.7624>

====================================== eX is yaw reading. //if (delt_t > 500) { // update LCD once per half-second independent of read rate
if (delt_t > 500) { // update serial output every 200ms if(SerialDebug) { Serial.print("ax = "); Serial.print((int)1000ax);
Serial.print(" ay = "); Serial.print((int)1000ay); Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg"); Serial.print("gx = "); Serial.print( gx, 2); Serial.print(" gy = "); Serial.print( gy, 2); Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s"); Serial.print("mx = "); Serial.print( (int)mx ); Serial.print(" my = "); Serial.print( (int)my ); Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG");

        Serial.print("q0 = "); Serial.print(q[0]);
        Serial.print(" qx = "); Serial.print(q[1]); 
        Serial.print(" qy = "); Serial.print(q[2]); 
        Serial.print(" qz = "); Serial.println(q[3]); 
      }               

      // 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   = atan2(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 = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
      roll  = atan2(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 *= 180.0f / PI;
      yaw   *= 180.0f / PI; 
      yaw   -= 13.8; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
      roll  *= 180.0f / PI;

      if(true) {
        // AutoX IMU msg format
        Serial.println("<qW:" + String(0.0, 4)
           + ",qX:" + String(0.0, 4)
           + ",qY:" + String(0.0, 4)
           + ",qZ:" + String(0.0, 4)
           **+ ",eX:" + String(yaw, 4)**
           + ",eY:" + String(pitch, 4)
           + ",eZ:" + String(roll, 4)
           + ">\r\n");

======= I was expecting yaw changed to 90 degrees every time I rotated it 90 degrees. It was about 77 degrees the 1st time, and later changed to 107 degrees later. After 360 degree rotation, the final yaw reading was very much inline with the original one. It's strange...

I didn't calibrate my sensor and I'm in bay area so I just reused your calibration data.

Also if I plan to printout yaw reading every 100ms instead of 500ms, will it work?

thanks a lot for your help and have a good day,

Steve

kriswiner commented 6 years ago

"I didn't calibrate my sensor "

then you could get any result...

On Mon, Oct 9, 2017 at 4:33 PM, scheng98 notifications@github.com wrote:

Hi Kris,

Thanks a lot for your nice coding work on MPU9250 IMU! I got your latest MPU9250BasicAHRS.ino code from GIT and ran it on my Arduino UNO with MPUT9250.

I made two small changes,

  1. serial board rate changed from 38400bps to 115200bps. the msg format was changed a little bit as well.
  2. comment out all LCD codes as I don't have them at all,

I got it working fine when rotating it 360 degrees anti-clockwise or clockwise. No Yaw drifting was found. However, I found an issue when rotating it 90 degree anti-clockwise, and repeat it 3 more times.

---- original position rate = 228.31 Hz ax = 2.99 ay = -74.89 az = -1076.97 mg gx = 0.04 gy = 0.12 gz = 0.16 deg/s mx = -6 my = -222 mz = -384 mG q0 = 0.00 qx = -0.04 qy = -1.00 qz = 0.03 qW:0.0000,qX:0.0000,qY:0.0000,qZ:0.0000,eX:161.9970,eY:- 0.0179,eZ:-176.1991

---- 90 Anti-clockwise rotate rate = 228.41 Hz ax = -4.94 ay = -75.56 az = -1083.07 mg gx = 0.05 gy = 0.15 gz = 0.14 deg/s mx = -281 my = -132 mz = -433 mG q0 = -0.02 qx = 0.51 qy = -0.86 qz = 0.03 qW:0.0000,qX:0.0000,qY:0.0000,qZ:0.0000,eX:-131.9031,eY: 0.1269,eZ:-175.9421

---- 90 Anti-clockwise rotate rate = 228.47 Hz ax = -6.04 ay = -78.55 az = -1079.22 mg gx = 0.03 gy = 0.11 gz = 0.08 deg/s mx = -185 my = 180 mz = -438 mG q0 = -0.03 qx = 0.94 qy = -0.34 qz = 0.01 qW:0.0000,qX:0.0000,qY:0.0000,qZ:0.0000,eX:-53.7203,eY: 0.2318,eZ:-175.8613

---- 90 Anti-clockwise rotate rate = 228.81 Hz ax = -0.12 ay = -77.27 az = -1074.28 mg gx = 0.03 gy = 0.05 gz = 0.09 deg/s mx = 108 my = 44 mz = -402 mG q0 = -0.03 qx = 0.82 qy = 0.57 qz = -0.02 qW:0.0000,qX:0.0000,qY:0.0000,qZ:0.0000,eX:55.0744,eY:0. 4858,eZ:-175.9031

---- 90 Anti-clockwise rotate rate = 227.37 Hz ax = 0.12 ay = -77.51 az = -1076.90 mg gx = 0.10 gy = 0.08 gz = 0.21 deg/s mx = -16 my = -236 mz = -380 mG q0 = -0.00 qx = 0.03 qy = 1.00 qz = -0.04 qW:0.0000,qX:0.0000,qY:0.0000,qZ:0.0000,eX:162.8931,eY: 0.0246,eZ:-175.7624

====================================== eX is yaw reading. //if (delt_t > 500) { // update LCD once per half-second independent of read rate if (delt_t > 500) { // update serial output every 200ms if(SerialDebug) { Serial.print("ax = "); Serial.print((int)1000 ax); Serial.print(" ay = "); Serial.print((int)1000ay); Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg"); Serial.print("gx = "); Serial.print( gx, 2); Serial.print(" gy = "); Serial.print( gy, 2); Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s"); Serial.print("mx = "); Serial.print( (int)mx ); Serial.print(" my = "); Serial.print( (int)my ); Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG");

    Serial.print("q0 = "); Serial.print(q[0]);
    Serial.print(" qx = "); Serial.print(q[1]);
    Serial.print(" qy = "); Serial.print(q[2]);
    Serial.print(" qz = "); Serial.println(q[3]);
  }

  // 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   = atan2(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 = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
  roll  = atan2(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 *= 180.0f / PI;
  yaw   *= 180.0f / PI;
  yaw   -= 13.8; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
  roll  *= 180.0f / PI;

  if(true) {
    // AutoX IMU msg format
    Serial.println("<qW:" + String(0.0, 4)
       + ",qX:" + String(0.0, 4)
       + ",qY:" + String(0.0, 4)
       + ",qZ:" + String(0.0, 4)
       **+ ",eX:" + String(yaw, 4)**
       + ",eY:" + String(pitch, 4)
       + ",eZ:" + String(roll, 4)
       + ">\r\n");

======= I was expecting yaw changed to 90 degrees every time I rotated it 90 degrees. It was about 77 degrees the 1st time, and later changed to 107 degrees later. After 360 degree rotation, the final yaw reading was very much inline with the original one. It's strange...

I didn't calibrate my sensor and I'm in bay area so I just reused your calibration data.

Also if I plan to printout yaw reading every 100ms instead of 500ms, will it work?

thanks a lot for your help and have a good day,

Steve

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU9250/issues/193, or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qlCsJbksBes9kEzG_jpupxZqYMNcks5sqq1LgaJpZM4PzL0- .

scheng98 commented 6 years ago

Got it. Shall I follow this instruction to calibrate? thanks!

Read this: https://github.com/kriswiner/MPU-6050/wiki/Simple-and-Effective-Magnetometer -Calibration and then copy the calibration functions from here: https://github.com/kriswiner/MPU-9250/blob/master/MPU9250_MS5637_AHRS_t3.ino

kriswiner commented 6 years ago

Good start, yes

On Mon, Oct 9, 2017 at 4:50 PM, scheng98 notifications@github.com wrote:

Got it. Shall I follow this instruction to calibrate? thanks!

Read this: https://github.com/kriswiner/MPU-6050/wiki/Simple-and- Effective-Magnetometer -Calibration and then copy the calibration functions from here: https://github.com/kriswiner/MPU-9250/blob/master/MPU9250_ MS5637_AHRS_t3.ino

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU9250/issues/193#issuecomment-335319844, or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qjrCbjceq2dD90-yIv_DDZBkAaD5ks5sqrFTgaJpZM4PzL0- .

scheng98 commented 6 years ago

Hmm... Still not good. I added "void magcalMPU9250(float dest1, float dest2) " to MPU9250BasicAHRS.ino with a few minor name changes. Also changed the loop() to use the new mag calibration data,

if NEEDED

  magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
  magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
  magbias[2] = +125.;  // User environmental x-axis correction in milliGauss

endif

  // Calculate the magnetometer values in milliGauss
  // Include factory calibration per data sheet and user environmental corrections
  mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
  my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];  
  mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];   
  mx *= magscale[0];
  my *= magscale[1];
  mz *= magscale[2];

then I did a calibration during set up, PU9250 is online... x-axis self test: acceleration trim within : 0.8% of factory value y-axis self test: acceleration trim within : 0.1% of factory value z-axis self test: acceleration trim within : -1.0% of factory value x-axis self test: gyration trim within : -2.2% of factory value y-axis self test: gyration trim within : -1.8% of factory value z-axis self test: gyration trim within : -0.1% of factory value MPU9250 initialized for active data mode.... AK8963 I AM 48 I should be 48 AK8963 initialized for active data mode.... Mag Calibration: Wave device in a figure eight until done! Mag Calibration done! AK8963 mag biases (mG) 0.00 0.00 0.00 0.88,0.78,1.73 X-Axis sensitivity adjustment value 1.17 Y-Axis sensitivity adjustment value 1.18 Z-Axis sensitivity adjustment value 1.14

I still get wierd orientation reading. The mag bias are all 0s. The mag scale is 0.88,0.78,1.73. Did I do anything wrong? Or I shall calibration acceleration/gyro as well?

thanks a lot for your help!

Steve

kriswiner commented 6 years ago

You are not doing the mag bias calibration correctly, should not be zeroes.

On Tue, Oct 10, 2017 at 12:55 PM, scheng98 notifications@github.com wrote:

Hmm... Still not good. I added "void magcalMPU9250(float dest1, float dest2) " to MPU9250BasicAHRS.ino with a few minor name changes. Also changed the loop() to use the new mag calibration data,

if NEEDED

magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated magbias[1] = +120.; // User environmental x-axis correction in milliGauss magbias[2] = +125.; // User environmental x-axis correction in milliGauss

endif

// Calculate the magnetometer values in milliGauss // Include factory calibration per data sheet and user environmental corrections mx = (float)magCount[0]mResmagCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set my = (float)magCount[1]mResmagCalibration[1] - magbias[1]; mz = (float)magCount[2]mResmagCalibration[2] - magbias[2]; mx = magscale[0]; my = magscale[1]; mz *= magscale[2];

then I did a calibration during set up, PU9250 is online... x-axis self test: acceleration trim within : 0.8% of factory value y-axis self test: acceleration trim within : 0.1% of factory value z-axis self test: acceleration trim within : -1.0% of factory value x-axis self test: gyration trim within : -2.2% of factory value y-axis self test: gyration trim within : -1.8% of factory value z-axis self test: gyration trim within : -0.1% of factory value MPU9250 initialized for active data mode.... AK8963 I AM 48 I should be 48 AK8963 initialized for active data mode.... Mag Calibration: Wave device in a figure eight until done! Mag Calibration done! AK8963 mag biases (mG) 0.00 0.00 0.00 0.88,0.78,1.73 X-Axis sensitivity adjustment value 1.17 Y-Axis sensitivity adjustment value 1.18 Z-Axis sensitivity adjustment value 1.14

I still get wierd orientation reading. The mag bias are all 0s. The mag scale is 0.88,0.78,1.73. Did I do anything wrong? Or I shall calibration acceleration/gyro as well?

thanks a lot for your help!

Steve

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU9250/issues/193#issuecomment-335588816, or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qnfsGl0Kw55dEndQ42k4TdpuESWfks5sq8uigaJpZM4PzL0- .

scheng98 commented 6 years ago

Hi Kris, it's strange. I tried quite a few times yesterday to calibrate it by waving digit 8. Still I got all 0.0s. I repeated same procedure on a Adafruit BNO055 and it was calibrated successfully.

Any other trick to do calibration on MPU9250?

thanks again,

Steve

kriswiner commented 6 years ago

I don't think you have the code properly implemented.

In other words the code has errors, something is missing, etc

On Wed, Oct 11, 2017 at 9:15 AM, scheng98 notifications@github.com wrote:

Hi Kris, it's strange. I tried quite a few times yesterday to calibrate it by waving digit 8. Still I got all 0.0s. I repeated same procedure on a Adafruit BNO055 and it was calibrated successfully.

Any other trick to do calibration on MPU9250?

thanks again,

Steve

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU9250/issues/193#issuecomment-335864078, or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qjmRb4zvLeIePBUjMenSKUBpvpM9ks5srOmogaJpZM4PzL0- .

JLark95 commented 6 years ago

Hey scheng98 could you post your modified .ino code? I don't have an LCD ether, if you could post it that would be great.