kriswiner / MPU9250

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

How to verify if calibration is OK #55

Open djsg opened 8 years ago

djsg commented 8 years ago

Hello Kris,

I have implemented your code for 9250 with STM32F401 MCU. The problem that I have is when sensor is flat and still, the calculated Q values are changing at a constant speed (big drifting). I suspect there is something wrong with the calibration, since wrong bias values would cause that. But how to verify calibration is OK or not. The following are the biases values I got from function mpu9250.calibrateMPU9250() gyro bias (-0.992366, -1.099237, 0.648855) accel bias (0.022339, 0.030212, 0.062500) Do you think they make sensse?

Thx & regards DJ

saedelman commented 6 years ago

I've hooked up the Pesky SENTral board but the returned pitch value appears to affected by both a rolling and pitching movement (e.g. rolling by 90DEG indcates a pitch of ~90DEG and pitching by 90DEG also indicates a pitch of ~90DEG). However, the returned roll value varies slightly from -2 to +2DEG. I'm using the same EM7180 setup() code as in https://github.com/kriswiner/EM7180_SENtral_sensor_hub/blob/master/EM7180_MPU9250_BMP280.ino

Note that I have a MPU9250 discrete device on the I2C bus in addition to your Pesky SENTral board. I am not using Passthrough for the SENTral board but grabbing Quaternions directly from the SENTral chip. The logging output related to the MPU9250 below is from initializing the discrete MPU9250.

Any ideas why roll appears locked? I have done no calibration on the SENTral board, as I assumed it has an auto calibration routine. Any suggestions?

EM7180 ROM Version: 0xe69 EM7180 RAM Version: 0xae23 EM7180 ProductID: 0x80 should be 0x80 EM7180 RevisionID: 0x2 should be 0x02 A barometer is installed A temperature sensor is installed EEPROM detected on the sensor bus! EEPROM uploaded config file! EEPROM upload successful! Beginning Parameter Adjustments Magnetometer Default Full Scale Range: +/-1000 uT Accelerometer Default Full Scale Range: +/-8g Gyroscope Default Full Scale Range: +/-2000 dps Magnetometer New Full Scale Range: +/-1000 uT Accelerometer New Full Scale Range: +/-8g Gyroscope New Full Scale Range: +/- 2000 dps EM7180 mag calibration completed EM7180 magnetic anomaly detected EM7180 new quaternion result EM7180 new mag result EM7180 new accel result EM7180 new gyro result EM7180 sensor status = 0 Actual MagRate = 100 Hz Actual AccelRate = 200 Hz Actual GyroRate = 200 Hz Actual BaroRate = 50 Hz x-axis self test: acceleration trim within : 1.794190 % of factory value y-axis self test: acceleration trim within : 4.287752 % of factory value z-axis self test: acceleration trim within : -0.931639 % of factory value x-axis self test: gyration trim within : 0.080680 % of factory value y-axis self test: gyration trim within : 0.610393 % of factory value z-axis self test: gyration trim within : -0.714925 % of factory value x gyro bias = -0.335878 y gyro bias = 0.290076 z gyro bias = 0.305344 x accel bias = 0.023987 y accel bias = 0.061768 z accel bias = 0.019714 Passthrough 22 MPU9250 initialized for active data mode.... AK8963 initialized for active data mode.... Accelerometer full-scale range = 2.000000 g Gyroscope full-scale range = 250.000000 deg/s Magnetometer resolution = 16 bits Magnetometer ODR = 100 Hz Accelerometer sensitivity is 16384.000000 LSB/g Gyroscope sensitivity is 131.072006 LSB/deg/s Magnetometer sensitivity is 0.776487 LSB/G bAHRS = 1 SY = 329 p = 87 r = 81 HY = 121 p = 105 r = -1 140 it/sec

kriswiner commented 6 years ago

Are you running both the EM7180-based board and the stand-alone MPU9250 at the same time?

Anyway, there is an autocalibration for the gyro and mag, but you have to actually move the board around to let the mag calibration work. It doesn;t auto calibrate by magic...

On Mon, Aug 27, 2018 at 2:09 PM Stephan Edelman notifications@github.com wrote:

I've hooked up the Pesky SENTral board but the returned pitch value appears to affected by both a rolling and pitching movement (e.g. rolling by 90DEG indcates a pitch of ~90DEG and pitching by 90DEG also indicates a pitch of ~90DEG). However, the returned roll value varies slightly from -2 to +2DEG. I'm using the same EM7180 setup() code as in https://github.com/kriswiner/EM7180_SENtral_sensor_hub/blob/master/EM7180_MPU9250_BMP280.ino

Note that I have a MPU9250 discrete device on the I2C bus in addition to your Pesky SENTral board. I am not using Passthrough for the SENTral board but grabbing Quaternions directly from the SENTral chip. The logging output related to the MPU9250 below is from initializing the discrete MPU9250.

Any ideas why roll appears locked? I have done no calibration on the SENTral board, as I assumed it has an auto calibration routine. Any suggestions?

EM7180 ROM Version: 0xe69 EM7180 RAM Version: 0xae23 EM7180 ProductID: 0x80 should be 0x80 EM7180 RevisionID: 0x2 should be 0x02 A barometer is installed A temperature sensor is installed EEPROM detected on the sensor bus! EEPROM uploaded config file! EEPROM upload successful! Beginning Parameter Adjustments Magnetometer Default Full Scale Range: +/-1000 uT Accelerometer Default Full Scale Range: +/-8g Gyroscope Default Full Scale Range: +/-2000 dps Magnetometer New Full Scale Range: +/-1000 uT Accelerometer New Full Scale Range: +/-8g Gyroscope New Full Scale Range: +/- 2000 dps EM7180 mag calibration completed EM7180 magnetic anomaly detected EM7180 new quaternion result EM7180 new mag result EM7180 new accel result EM7180 new gyro result EM7180 sensor status = 0 Actual MagRate = 100 Hz Actual AccelRate = 200 Hz Actual GyroRate = 200 Hz Actual BaroRate = 50 Hz x-axis self test: acceleration trim within : 1.794190 % of factory value y-axis self test: acceleration trim within : 4.287752 % of factory value z-axis self test: acceleration trim within : -0.931639 % of factory value x-axis self test: gyration trim within : 0.080680 % of factory value y-axis self test: gyration trim within : 0.610393 % of factory value z-axis self test: gyration trim within : -0.714925 % of factory value x gyro bias = -0.335878 y gyro bias = 0.290076 z gyro bias = 0.305344 x accel bias = 0.023987 y accel bias = 0.061768 z accel bias = 0.019714 Passthrough 22 MPU9250 initialized for active data mode.... AK8963 initialized for active data mode.... Accelerometer full-scale range = 2.000000 g Gyroscope full-scale range = 250.000000 deg/s Magnetometer resolution = 16 bits Magnetometer ODR = 100 Hz Accelerometer sensitivity is 16384.000000 LSB/g Gyroscope sensitivity is 131.072006 LSB/deg/s Magnetometer sensitivity is 0.776487 LSB/G bAHRS = 1 SY = 329 p = 87 r = 81 HY = 121 p = 105 r = -1 140 it/sec

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

saedelman commented 6 years ago

Yes, I am running the EM7180 board and the standalone MPU9250 at the same time. I made sure that state variables weren't being overwritten, etc. The standalone MPU9250 and Mahony give decent and repeatable results, but the EM7180 does not show any roll. I noticed that the EM7180 status (above) indicates that a "magnetic anomaly detected", so the mag is definitely uncalibrated. I've moved the EM7180 in a figure-8, but I am unsure if it calibrates while it is normally running or if I have to call a specific calibration routine. I'll do some more digging.

kriswiner commented 6 years ago

No, it calibrates the mag on the fly. there is a warm start parameter save function so that the mag and fusion parameters can be saved for use in subsequent start ups. You can judge whether the mag is calibrated by flipping the sensor up and down and checking that the z-component is equal and opposite.

On Mon, Aug 27, 2018 at 3:55 PM Stephan Edelman notifications@github.com wrote:

Yes, I am running the EM7180 board and the standalone MPU9250 at the same time. I made sure that state variables weren't being overwritten, etc. The standalone MPU9250 and Mahony give decent and repeatable results, but the EM7180 does not show any roll. I noticed that the EM7180 status (above) indicates that a "magnetic anomaly detected", so the mag is definitely uncalibrated. I've moved the EM7180 in a figure-8, but I am unsure if it calibrates while it is normally running or if I have to call a specific calibration routine. I'll do some more digging.

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

saedelman commented 6 years ago

Good point. I'll give that a try.

saedelman commented 6 years ago

I'm getting better results out of Mahony with discrete MPU9250 than the SENTral fusion board even after waving the board in an 8-pattern for a minute or two. Although absolute heading appears more accurate for the SENTral board when it is laying horizontal, if I pitch this board 90DEG up so that it is vertical without a change in heading, the indicated heading is almost 50DEG off and is "walking". The heading change with Mahony is less than 1DEG in this instance. I'm using EM7180_MPU9250_BMP280.ino without any functional changes.

kriswiner commented 6 years ago

Of course, this is counter to all of our testing and expectations. Not sure why this would be the case unless the sensors are not calibrated (which seems likely).

On Tue, Aug 28, 2018 at 8:02 AM Stephan Edelman notifications@github.com wrote:

I'm getting better results out of Mahony with discrete MPU9250 than the SENTral fusion board even after waving the board in an 8-pattern for a minute or two. Although absolute heading appears more accurate for the SENTral board when it is laying horizontal, if I pitch this board 90DEG up so that it is vertical without a change in heading, the indicated heading is almost 50DEG off and is "walking". The heading change with Mahony is less than 1DEG in this instance. I'm using EM7180_MPU9250_BMP280.ino without any functional changes.

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

kriswiner commented 6 years ago

We have some guidance of calibration of the accel here https://github.com/kriswiner/EM7180_SENtral_sensor_hub/wiki/F.--Magnetometer-and-Accelerometer-Calibration and mag here https://github.com/kriswiner/MPU6050/wiki/Simple-and-Effective-Magnetometer-Calibration. It is easy to check if these are calibrated just by turning the board top up them bottom up, etc as I suggested. if the sensors are not calibrated, you can get any result.

In any case, if you are happy with the Mahoney results with your board I would suggest you just use this.

On Tue, Aug 28, 2018 at 8:52 AM Tlera Corporation tleracorp@gmail.com wrote:

Of course, this is counter to all of our testing and expectations. Not sure why this would be the case unless the sensors are not calibrated (which seems likely).

On Tue, Aug 28, 2018 at 8:02 AM Stephan Edelman notifications@github.com wrote:

I'm getting better results out of Mahony with discrete MPU9250 than the SENTral fusion board even after waving the board in an 8-pattern for a minute or two. Although absolute heading appears more accurate for the SENTral board when it is laying horizontal, if I pitch this board 90DEG up so that it is vertical without a change in heading, the indicated heading is almost 50DEG off and is "walking". The heading change with Mahony is less than 1DEG in this instance. I'm using EM7180_MPU9250_BMP280.ino without any functional changes.

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

kriswiner commented 6 years ago

And just to make sure, you are using the "hardware sensor fusion" quaternions and not the Madgwick output done on the host presumably.

It is possible that the offset biases are so bad for the odd MPU9250 that it takes a bit more work to get the sensor calibrated and get good results from the EM7180 but even in the worst cases we have tested it is still possible to get ~2 degree heading accuracy from these.

In the best cases (with well behaved sensors) we can get ~1 degree heading accuracy.

All sensors will show this indeterminate heading "error" when the pitch or roll is at 90 degrees, this is just a problem of determining which direction is South when standing on the North pole.

On Tue, Aug 28, 2018 at 8:02 AM Stephan Edelman notifications@github.com wrote:

I'm getting better results out of Mahony with discrete MPU9250 than the SENTral fusion board even after waving the board in an 8-pattern for a minute or two. Although absolute heading appears more accurate for the SENTral board when it is laying horizontal, if I pitch this board 90DEG up so that it is vertical without a change in heading, the indicated heading is almost 50DEG off and is "walking". The heading change with Mahony is less than 1DEG in this instance. I'm using EM7180_MPU9250_BMP280.ino without any functional changes.

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

saedelman commented 6 years ago

The Mahony results look decent but I would like to qualify these with the "gold" standard so I can optimize. In particular, I want to adjust the Kp/Ki values in Mahony to adjust for our particular update rate and use case (which is more constrained in some ways compared to a drone) and would like to be able to compare with the SENTRal that has a much higher update and convergence rate. So, I'm going to keep at it until I have this SENTral working right.

With respect to Mahony, as our update rate is relatively slow (100 iterations of Mahony per second), the "deltat" value is large compared to typical/suggested update rates and as a result, I see more "wobble" in the convergence to steady state when I iterate Mahony in a for...next loop 5 or 10 times while keeping the accel,gyro,mag data constant. This suggests to me that Mahony's error prediction is actually quite good and re-integrating over the same deltat when the data hasn't changed is actually detrimental to convergence speed. I am assuming that you are not seeing this as the time period over which you are integrating is so small because of the high update rate.

I have found that the following works the best and minimizes the time to steady state:

if (accel_ready && gyro_ready && mag_ready) MahonyUpdate(...)

Which means that Mahony is only called when there is new data and only once. It seems counter intuitive that this works better than calling MahonyUpdate(..) regardless of new data, but that is what I am seeing for larger deltat.

kriswiner commented 6 years ago

Actually this is what I expect. I mentioned the subtle error we are making and this is the heart of it. If one calls the fusion filter again with the same data this is equivalent to "telling" the filter that the device has rotated again by whatever the gyro data are. This is wrong. But it works because the subsequent deltats are very small compared to the first one. So your scheme is actually closer to being correct. You could iterate further with the gyro data zeroed out, but if a single iteration works, I would stick with this.

I think 100 Hz is rather low for an update rate but it will depend on your application. We generally see best results with high sample rates (~1 kHz for gyro and accel) and fusion rates at least as high as this.

I am not sure hwat is happening with the SENtral, but keep at it and we might learn something...

On Tue, Aug 28, 2018 at 9:36 AM Stephan Edelman notifications@github.com wrote:

The Mahony results look decent but I would like to qualify these with the "gold" standard so I can optimize. In particular, I want to adjust the Kp/Ki values in Mahony to adjust for our particular update rate and use case (which is more constrained in some ways compared to a drone) and would like to be able to compare with the SENTRal that has a much higher update and convergence rate. So, I'm going to keep at it until I have this SENTral working right.

With respect to Mahony, as our update rate is relatively slow (100 iterations of Mahony per second), the "deltat" value is large compared to typical/suggested update rates and as a result, I see more "wobble" in the convergence to steady state when I iterate Mahony in a for...next loop 5 or 10 times while keeping the accel,gyro,mag data constant. This suggests to me that Mahony's error prediction is actually quite good and re-integrating over the same deltat when the data hasn't changed is actually detrimental to convergence speed. I am assuming that you are not seeing this as the time period over which you are integrating is so small because of the high update rate.

I have found that the following works the best and minimizes the time to steady state:

if (accel_ready && gyro_ready && mag_ready) MahonyUpdate(...)

Which means that Mahony is only called when there is new data and only once. It seems counter intuitive that this works better than calling MahonyUpdate(..) regardless of new data, but that is what I am seeing for larger deltat.

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

saedelman commented 6 years ago

In our application I only need to be able to track a standard rate turn (3DEG/second) and so far it looks like I can do this without too much lag using Mahony and the 100 iterations/second. Our vehicle cannot move at the rates a drone can so the update rate seems workable.

With respect to Mahony, I came across some comments on an RC drone forum about initializing q[0..3] based on current accel, gyro and mag readings so that the initial startup takes less time to converge. Right now, it takes about 4-6 seconds on average to converge to steady state when the host is started. There is a YouTube video (https://youtu.be/OLD_4u4SRAE) illustrating this. I can't find any code out there that does this, have you seen this before?

I'm going to keep at it and get the SENTral solution working. I'll let you know if I fin anything obvious.

saedelman commented 6 years ago

I'm still fighting with the SENTral board, heading still seems to be "walking" for 5 seconds each time I change orientation but then remains stable. I have it glued onto a 4-layer 31um PCB that has the host CPU. I've placed the board in a corner where all copper is removed on all layers and clear of copper for about 4mm all around the SENTral board. I'm using wirewrap wire soldered to the pins to route the power and I2C signals to the host CPU. Mahony seems to track it almost instantly while talking to the onboard MPU9250. Still not sure what's going on.

saedelman commented 6 years ago

With respect to lines 1472-1474 in https://github.com/kriswiner/EM7180_SENtral_sensor_hub/blob/master/EM7180_MPU9250_BMP280.ino why are the accelerometer factory trim values not included in the bias that is returned to the main program? Should these not be included? Should these lines not be:

dest2[0] = (float)accel_bias_reg[0] * 8.0f/(float)accelsensitivity;
dest2[1] = (float)accel_bias_reg[1] * 8.0f/(float)accelsensitivity;
dest2[2] = (float)accel_bias_reg[2] * 8.0f/(float)accelsensitivity;

instead?

kriswiner commented 6 years ago

Where do you find the accel factory trim values?

On Tue, Sep 4, 2018 at 8:42 AM Stephan Edelman notifications@github.com wrote:

With respect to lines 1472-1474 in https://github.com/kriswiner/EM7180_SENtral_sensor_hub/blob/master/EM7180_MPU9250_BMP280.ino why are the accelerometer factory trim values not included in the bias that is returned to the main program? Should these not be included?

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

saedelman commented 6 years ago

I assumed the manufacturer would initially populate these values based on process/testing?

  readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
  readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);
  readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
kriswiner commented 6 years ago

No. These default to zero.

On Tue, Sep 4, 2018 at 8:54 AM Stephan Edelman notifications@github.com wrote:

I assumed the manufacturer would initially populate these values based on process/testing?

readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]); readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);

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

saedelman commented 6 years ago

Ah, I see. I didn't realize these were user-defined trim registers, rather than factory trim registers.

kriswiner commented 6 years ago

Yes, but the LSB is a temperature compensation bit which has to be preserved. I could never get this to work properly so I do all of the accel compensation in the main loop. I do use the gyro offset registers though.

On the SENtral I am not sure what to say. What value of quaternion update rate are you using? I would recommend you set qratedivisor to 0. I would also recommend you use at least 200 Hz gyro rate but 1 kHz is likely to produce better results. Maybe Greg has some ideas to improve your experience. We find the SENtral responds rapidly to chages with little overshoot or settling and very stable heading. Nit quite sure what is happening in your case.

On Tue, Sep 4, 2018 at 9:01 AM Stephan Edelman notifications@github.com wrote:

Ah, I see. I didn't realize these were user-defined trim registers, rather than factory trim registers.

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

saedelman commented 6 years ago

I'm using the following:

Actual MagRate = 100 Hz Actual AccelRate = 200 Hz Actual GyroRate = 200 Hz Actual BaroRate = 50 Hz

I've changed the QrateDivisor to 0 and increased the Gyro Rate to 1000Hz. This seems to have increased the Accel rate also.

Actual MagRate = 100 Hz Actual AccelRate = 1000 Hz Actual GyroRate = 1000 Hz Actual BaroRate = 50 Hz

it is still walking the heading by about 0.5 deg/second for about 5-6 seconds.

kriswiner commented 6 years ago

The quaternion rate is limited to 400 Hz so for gyro rates of 1000 Hz it might be better to set qrate div to 1, but this will likely not make a difference to the walking behavior. Not sure I understand what this is...

On Tue, Sep 4, 2018 at 9:22 AM Stephan Edelman notifications@github.com wrote:

I'm using the following:

Actual MagRate = 100 Hz Actual AccelRate = 200 Hz Actual GyroRate = 200 Hz Actual BaroRate = 50 Hz

I've changed the QrateDivisor to 0 and increased the Gyro Rate to 1000Hz. This seems to have increased the Accel rate also.

Actual MagRate = 100 Hz Actual AccelRate = 1000 Hz Actual GyroRate = 1000 Hz Actual BaroRate = 50 Hz

it is still walking the heading by about 0.5 deg/second for about 5-6 seconds.

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

saedelman commented 6 years ago

I'm not sure it is relevant, but I am also seeing that the pitch value does not behave well when I place the unit vertical. Right now, I see a minimal value when the board is perfectly vertical, but tilting forward increases the pitch and tilting backward also increases the pitch value. I would have expected it to flip sign. Adding 90, 180, etc. does not change this behaviour.

saedelman commented 6 years ago

I'm just going to order a new SENTral board from you. That will tell me if the issue is software or otherwise. If you don't mind testing the board before you ship it, that would be great.

kriswiner commented 6 years ago

I test them all for function. In this case, I will ask my colleague Greg Tomasch to test erfromance as well and send that one to you so we know it behaves as we expect. This might just be a matter of differences in expectation.

In the end, if the Mahony filter and plain MPU9250 gives you better results in your application I would go with this.

Are you able to characterize the heading accuracy you are getting from the Mahony + MPU9250 solution? Usually a simple 90-degree corner turning test is sufficient to tell.

On Tue, Sep 4, 2018 at 9:32 AM Stephan Edelman notifications@github.com wrote:

I'm just going to order a new SENTral board from you. That will tell me if the issue is software or otherwise. If you don't mind testing the board before you ship it, that would be great.

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

saedelman commented 6 years ago

I think my problem is with the quaternion to Euler conversion. That explains why my heading is walking. It also explains why my pitch is not sensible. I believe the quaternions are returned differently than Mahony. Let me check this and report back.

I've used Magneto v1.2 (https://sites.google.com/site/sailboatinstruments1/) to calculate the bias and transform for both the magnetometer and the accelerometer and get very good repeatability and a heading error between 4-5 degrees. As I am getting good repeatability, I've plotted the error vs angle and applied a function to correct the heading within 1 degree. I'm hoping that with the onboard GPS and world-magnetic-model (WMM), I can normalize the heading and continue to apply the error correction even with changing declination.

kriswiner commented 6 years ago

Yes, the quaternion order is likely to be different. You could simply read Euler angles instead of quternions directly from the EM7180 by changing a register bit.

On Tue, Sep 4, 2018 at 9:57 AM Stephan Edelman notifications@github.com wrote:

I think my problem is with the quaternion to Euler conversion. That explains why my heading is walking. It also explains why my pitch is not sensible. I believe the quaternions are returned differently than Mahony. Let me check this and report back.

I've used Magneto v1.2 ( https://sites.google.com/site/sailboatinstruments1/) to calculate the bias and transform for both the magnetometer and the accelerometer and get very good repeatability and a heading error between 4-5 degrees. As I am getting good repeatability, I've plotted the error vs angle and applied a function to correct the heading within 1 degree. I'm hoping that with the onboard GPS and world-magnetic-model (WMM), I can normalize the heading and continue to apply the error correction even with changing declination.

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

saedelman commented 6 years ago

I tried the quaternion input order that Greg forwarded to me but got the same results so I turned on Euler angles (0x04 for AlgorithmControl) and I'm getting the exact same output as previously. The heading walks and I just noticed that the heading is also affected up to 50DEG by pitch when Euler output is enabled. Something isn't right so I have ordered another SENTral board just to eliminate any possible hardware issue.

kriswiner commented 6 years ago

Might be a good idea if we take a look at the relevant code you are using, this is an unexpected result. Are you sure you are not inadvertently mixing the host-based with the EM7180-based quaternions/Euler angles?

On Tue, Sep 4, 2018 at 12:10 PM Stephan Edelman notifications@github.com wrote:

I tried the quaternion input order that Greg forwarded to me but got the same results so I turned on Euler angles (0x04 for AlgorithmControl) and I'm getting the exact same output as previously. The heading walks and I just noticed that the heading is also affected up to 50DEG by pitch when Euler output is enabled. Something isn't right so I have ordered another SENTral board just to eliminate any possible hardware issue.

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

saedelman commented 6 years ago

Found the problem. The gyro is returning zero for all axis from the SENTral board when getting the raw data (readBytes(EM7180_ADDRESS, EM7180_GX, 6, &rawData[0])). That explains a lot. Probably means that I still have an overvoltage issue. It appears the Gyro is the most sensitive to overvoltage. I'm going to replace the MPU9250 on the SENTRal board and try again.

-0.320250 = 0.000000 (gx) -0.045935 = 0.000000 (gy) 0.427078 = 0.000000 (gz)

kriswiner commented 6 years ago

Are you saying application of >3.6 V somehow/sometime on your board broke the gyro? This is possible. Easiest way to tell is put the EM7180 into passthrough mode and read the gyro data.

Do you still want the second USFS you ordered or shall I cancel the order?

On Tue, Sep 4, 2018 at 12:24 PM Stephan Edelman notifications@github.com wrote:

Found the problem. The gyro is returning zero for all axis from the SENTral board when getting the raw data (readBytes(EM7180_ADDRESS, EM7180_GX, 6, &rawData[0])). That explains a lot. Probably means that I still have an overvoltage issue. It appears the Gyro is the most sensitive to overvoltage. I'm going to replace the MPU9250 on the SENTRal board and try again.

-0.320250 = 0.000000 (gx) -0.045935 = 0.000000 (gy) 0.427078 = 0.000000 (gz)

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

saedelman commented 6 years ago

Yes, that has happened previously with the discrete MPU9250 on my board. The gyro seems very sensitive to overvoltage. I thought I had corrected the issue, but it looks like I may not corrected it. I managed to replace the MPU9250 on the SENTRal board and everything appears to be working correctly. No more walking and the heading appears rock solid. No need to cancel the order. I will keep it as a spare. sentralboard

saedelman commented 6 years ago

Well, this blows...Just received the following below regarding MPU9250 end-of-life announcement. Last order Feb 9, 2019. Replacement is ICM-20948, a part with VDDIO of 1.8v.

https://store.invensense.com/datasheets/invensense/EOL-MPU-9250-v1-002.pdf

kriswiner commented 6 years ago

Yup, not unexpected. This is why we have already developed a replacement using ST sensors...

On Mon, Sep 10, 2018 at 10:48 AM Stephan Edelman notifications@github.com wrote:

Well, this blows...Just received the following below regarding MPU9250 end-of-life announcement. Last order Feb 9, 2019. Replacement is ICM-20948, a part with VDDIO of 1.8v.

https://store.invensense.com/datasheets/invensense/EOL-MPU-9250-v1-002.pdf

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

saedelman commented 6 years ago

It was a little sooner than I expected. Which ST sensors are you using? Have you developed a USFS board for this also?

kriswiner commented 6 years ago

https://www.tindie.com/products/onehorse/ultimate-sensor-fusion-solution-lsm6dsm--lis2md/?pt=ac_prod_search

On Mon, Sep 10, 2018 at 11:02 AM Stephan Edelman notifications@github.com wrote:

It was a little sooner than I expected. Which ST sensors are you using? Have you developed a USFS board for this also?

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