uNav INS

You won't see it in the debug - the delay is enum in the MS5637 library.

I meant "details in a quick scan" of this thread to confirm if it is i2c? - didn't look at the library. This would be a place TeensyThreads could help - but only if it is on a separate i2c bus? ... same with the i2c_t3 dma/int usage in normal sketch - that would really hog up the i2c timeslices.

Bummer if it doesn't have an INT to say 'data ready' - that works well on the IMU to not have to wait for a read to complete.
 
Tim
"TeensyThreads could help - but only if it is on a separate i2c bus?" Yep - that was what I was thinking. There aren't many 9250 breakout boards out there that have a pressure sensor on board.

Don
Nice work on the calibration routine. Can't wait to see it.

Mike
 
Here's an interesting thing I'm seeing with the new cal analysis tool. My X mag sensor seems to have noise spikes. Here's a sample Sigma calculation using buffer size of 2048:

Static IMU Sensor Sigma Values:
axb Sigma (m/ss): 0.007790
ayb Sigma (m/ss): 0.007829
azb Sigma (m/ss): 0.012178
gxb Sigma (deg/s): 0.000711
gyb Sigma (deg/s): 0.000599
gzb Sigma (deg/s): 0.000651
hxb Sigma (uT): 2.628827
hyb Sigma (uT): 0.760816
hzb Sigma (uT): 0.745304

If I set the buffer size down to, say 64, I get some X mag Sigma calculations that look normal, and some that are WAY high. So I think that's telling me I'm getting periodic spikes (large or small) on my X mag reading. I guess I'll do some troubleshooting, see if I can capture it. Either way, I'm guessing we'll need to do some pre-editing (pre-checks) of sensor values before we feed them to the filter, otherwise we could periodically pass noise spikes or erroneous readings to the EKF.

Thinking I can use the min and max features of the circular buffer to see if there are spikes, and if so, how big.

Don
 
Thanks for posting Don. Will have to give it a try. Probably later tonight.

Looks real interesting.

Mike
 
Hi Don
Just tried your calibration routine. A lot to like but having a problem. Doesn't want to save calibrations to eeprom or the imu it seems. Here is what I get after running all calibrations on accel, mag and gyro:
Code:
Accel X Bias:         0.044415
Accel X Scale Factor: 1.000512
Accel Y Bias:         0.086112
Accel Y Scale Factor: 1.002414
Accel Z Bias:         -0.346097
Accel Z Scale Factor: 1.000565
Gyro X Bias:          -0.002217
Gyro Y Bias:          0.006990
Gyro Z Bias:          -0.011183
Mag X Bias:           8.755037
Mag X Scale Factor:   0.976082
Mag Y Bias:           -2.065775
Mag Y Scale Factor:   0.997860
Mag Z Bias:           -31.500816
Mag Z Scale Factor:   1.027378
After I save the values to eeprom and do a "d" and it only displays the default values. After I run the actual sketch I and it prints out the cals on startup its again showing only the default values.

I just started going through the code but will probably have to finish later. Just thought you should know.

Mike
 
Just did a simple test and I'm seeing it load. Here's what I did:
1. 'z' to zero everything out
2. 'd' to verify everything reset
3. 's' to do static cal
4. 'e' to load into eeprom
5. 'i' to load into MPU

Static Calibration Bias Values:
axb: -0.042200
ayb: 0.415340
azb: -0.564213
gxb: 0.019995
gyb: -0.017028
gzb: -0.001667
hxb: 22.567411
hyb: 5.940891
hzb: -35.756413

Static Calibration Values Saved to EEPROM

Teensy Accel and Gyro EEPROM Biases:
Current Accel Biases (mss) and Scale Factors:
-0.042200, 1.000000
0.415340, 1.000000
-0.564213, 1.000000
Current Mag Biases (uT) and Scale Factors:
22.567411, 1.000000
5.940891, 1.000000
-35.756413, 1.000000

IMU Biases:
Accel X Bias: -0.042200
Accel X Scale Factor: 1.000000
Accel Y Bias: 0.415340
Accel Y Scale Factor: 1.000000
Accel Z Bias: -0.564213
Accel Z Scale Factor: 1.000000
Gyro X Bias: 0.019995
Gyro Y Bias: -0.017028
Gyro Z Bias: -0.001667
Mag X Bias: 22.567411
Mag X Scale Factor: 1.000000
Mag Y Bias: 5.940891
Mag Y Scale Factor: 1.000000
Mag Z Bias: -35.756413
Mag Z Scale Factor: 1.000000

The procedure will be bit different for MPU9250 Library cal, I'll try that next...

Don
 
I see what you're saying, just did a MPU9250 Library accel cal, and it didn't load to eeprom when requested. I'll check the code later tonight. I think it's a simple fix, think I'm not passing the variable correctly to the load EEPROM routine following a MPU9250 Library cal, but I'm doing it correctly for the static cal. Thx for catching it.

Teensy Accel and Gyro EEPROM Biases:
Current Accel Biases (mss) and Scale Factors:
0.000000, 1.000000
0.000000, 1.000000
0.000000, 1.000000
Current Mag Biases (uT) and Scale Factors:
0.000000, 1.000000
0.000000, 1.000000
0.000000, 1.000000

IMU Biases:
Accel X Bias: 0.071219
Accel X Scale Factor: 1.001855
Accel Y Bias: 0.477212
Accel Y Scale Factor: 0.999564
Accel Z Bias: -0.165475
Accel Z Scale Factor: 1.010679
Gyro X Bias: 0.000000
Gyro Y Bias: 0.000000
Gyro Z Bias: 0.000000
Mag X Bias: 0.000000
Mag X Scale Factor: 1.000000
Mag Y Bias: 0.000000
Mag Y Scale Factor: 1.000000
Mag Z Bias: 0.000000
Mag Z Scale Factor: 1.000000
 
I tried that method as well with the same results. Can you do me one favor. After your last step if you do a "d" again you should be getting back the values that were stored, correct. That is where the problem I have comes in.

I will check again after diner :)
 
When you get a chance, try the 'r' option and let me know if you're seeing consistent values. I've added variables for the buffer size now, as it's nice to be able to change that quickly.

I'll do another release tonight with the eeprom load fix, hopefully, after dinner...

Don
 
Mike,
Actually think it's working. To load values to eeprom after a MPU9250 Library calibration (following 'a', 'g', or 'm') you use the 'l' command instead of the 'e' command. The 'e' command is for loading static cal values into eeprom. It's a bit confusing, but a bit challenging without an easier GUI!

Here's what I see doing an 'a' cal followed by an 'l' load command:

Teensy Accel and Gyro EEPROM Biases:
Current Accel Biases (mss) and Scale Factors:
0.000000, 1.000000
0.000000, 1.000000
0.000000, 1.000000
Current Mag Biases (uT) and Scale Factors:
0.000000, 1.000000
0.000000, 1.000000
0.000000, 1.000000

IMU Biases:
Accel X Bias: 0.071219
Accel X Scale Factor: 1.001855
Accel Y Bias: 0.477212
Accel Y Scale Factor: 0.999564
Accel Z Bias: -0.165475
Accel Z Scale Factor: 1.010679
Gyro X Bias: 0.000000
Gyro Y Bias: 0.000000
Gyro Z Bias: 0.000000
Mag X Bias: 0.000000
Mag X Scale Factor: 1.000000
Mag Y Bias: 0.000000
Mag Y Scale Factor: 1.000000
Mag Z Bias: 0.000000
Mag Z Scale Factor: 1.000000

Static Calibration Values Saved to EEPROM

MPU9250 Lib Calibration Values Saved to EEPROM

Teensy Accel and Gyro EEPROM Biases:
Current Accel Biases (mss) and Scale Factors:
0.071219, 1.001855
0.477212, 0.999564
-0.165475, 1.010679
Current Mag Biases (uT) and Scale Factors:
0.000000, 1.000000
0.000000, 1.000000
0.000000, 1.000000
 
A little off between runs of 'r'. Here is a copy of the data runs:
Code:
axb Sigma (m/ss):  0.009827
ayb Sigma (m/ss):  0.007082
azb Sigma (m/ss):  0.013256
gxb Sigma (deg/s): 0.000703
gyb Sigma (deg/s): 0.000409
gzb Sigma (deg/s): 0.000749
hxb Sigma (uT):    0.844880
hyb Sigma (uT):    2.908176
hzb Sigma (uT):    0.797112
 
Static IMU Sensor Sigma Values:
axb Sigma (m/ss):  0.012116
ayb Sigma (m/ss):  0.008733
azb Sigma (m/ss):  0.012601
gxb Sigma (deg/s): 0.000788
gyb Sigma (deg/s): 0.000717
gzb Sigma (deg/s): 0.000647
hxb Sigma (uT):    0.786440
hyb Sigma (uT):    3.000012
hzb Sigma (uT):    3.880017
 
Static IMU Sensor Sigma Values:
axb Sigma (m/ss):  0.007050
ayb Sigma (m/ss):  0.008316
azb Sigma (m/ss):  0.014569
gxb Sigma (deg/s): 0.000811
gyb Sigma (deg/s): 0.000476
gzb Sigma (deg/s): 0.000854
hxb Sigma (uT):    0.867680
hyb Sigma (uT):    2.874687
hzb Sigma (uT):    0.616143
 
Static IMU Sensor Sigma Values:
axb Sigma (m/ss):  0.008588
ayb Sigma (m/ss):  0.008830
azb Sigma (m/ss):  0.013177
gxb Sigma (deg/s): 0.000692
gyb Sigma (deg/s): 0.000528
gzb Sigma (deg/s): 0.000577
hxb Sigma (uT):    0.706945
hyb Sigma (uT):    0.680673
hzb Sigma (uT):    2.769747

Think your CBs may need to be bigger. The buffer size should fix that. By the way saving it to the imu works fine. The values do look zeroed out correctly.

Mike
 
Below is Ver2. I added the ability to change the buffer size via variable in the main routine, and I added min and max outputs when you run 'r'.

Note this result... interesting. The circular buffer pulled in bad IMU readings on two different sensors (see below). Apparently az output a 0 at least once, and the hx mag read a low near 0.01 whereas the range should be between 45 to 50 or so. So thinking this definitely means we may need some sort of IMU sensor error checking before data goes in to the EKF. What do y'all think? Pretty telling...

Static IMU Sensor Sigma Values:
axb Sigma (m/ss): 0.007172 axMax: 0.028732 axMin: 0.435767
ayb Sigma (m/ss): 0.006689 ayMax: 0.469287 ayMin: 0.435767
azb Sigma (m/ss): 0.012097 azMax: 0.000000 azMin: -10.400942
gxb Sigma (deg/s): 0.000735 gxMax: 0.001551 gxMin: -0.002310
gyb Sigma (deg/s): 0.000630 gyMax: 0.001941 gyMin: -0.001388
gzb Sigma (deg/s): 0.000628 gzMax: 0.001899 gzMin: -0.001696
hxb Sigma (uT): 1.635229 hxMax: 49.363297 hxMin: 0.182152
hyb Sigma (uT): 0.840308 hyMax: 14.754344 hyMin: 10.018381
hzb Sigma (uT): 0.729853 hzMax: 8.434066 hzMin: 4.392743


View attachment CalAnalysisMPU9250Ver2.zip
 
Interesting - I wonder if "Imu.readSensor();" returned an error? Noted the other day it returns -1 on failure to read - it returns before altering stored values. But checking that return would be the first step before using any data after a reported failed read.

from \libraries\MPU9250\MPU9250.cpp::
Code:
int MPU9250::readSensor() {
  _useSPIHS = true; // use the high speed SPI for data readout
  
  // grab the data from the MPU9250
  if (readRegisters(ACCEL_OUT, 21, _buffer) < 0) {
    return -1;
  }

Cool to add the '?' - help command to reprint the commands shown in setup as a func() to re-print them.

I still need to rebuild my breadboard to a stable perfboard unit before I start handling it much.
 
Great idea Tim, would make for an easy first check.

I've got some of the Adafruit Perma-Proto boards arriving this week, so I'll be soldering up one of those. I also have one of Brian's Marmot boards to try out when I get a chance. I've definitely had signal issues in the past with these regular breadboards...

Don
 
Morning Don,

Haven't tried v2 yet but I will this morning. Just wanted to let you know think I found the problem with the eeprom save. You have to save it to the IMU library first then save it to the eeprom. The write to eeprom saves the data using library calls for the biases and offsets.

Mike
 
Mike,
It depends on whether you're using option 'e' (load static cal data to EEPROM) or option 'l' (load MPU9250 Library cal data to EEPROM).

When a static cal is calculated, scale factors are not solved for, so they are left at 1. So just the statically-solved-for biases are loaded to EEPROM when 'e' is selected. When a cal is done using the MPU9250 Library cal options ('a','g','m'), the user needs to use option 'l' to then load the MPU9250 Library biases and scale factors to EEPROM.

These seem to be working as intended when I run them, but I'll double-check again today in case I missed something. Since the order of the commands can be confusing, I'm going to add several Use Cases to my PDF to show the order that the commands need to be sent.

Sorry about any confusion with the intent of the commands. I tried to keep each command succinct, but it means they must be done in the right order. Will get to adding several Use Cases later today!

Don
 
Hi Don,

My last post was on eeprom save was how you would do it for static cals. For the library version, after calibration of accel and mag is complete you just have to hit "l". Then the eeprom is updated.

By the way a couple of interesting things to note:
1. the static cal seems to be better than the library version but when I load the values in the sketch that's where the problems start. I start getting assert errors or the transfers start hanging after a while the transfers just hang. When it runs I get pitch close to 0, about 5 deg and roll 0.
2. The library cal runs fine but gives a roll of about 20degs. Can't win. So I
3. Something screwy going on with cal.
4. Had to reduce the size of the CB to 1024 as 2048 would overflow my T3.5. If using a T3.2 would have to be even less

More to follow..... going to look at Onehorses calibration methodology next.
 
Good points. If we can figure out some of these cal issues, and make sure we're getting good quality data, I'm guessing we'll see some good stuff with both the EKF w/o mag and the EKF w/ mag!

I'll pull together my Use Cases later today.

Am also curious to see how different the MPU9250 Lib cals are from my static cals. I'm expecting the gyro and accel cals to be about the same either way, but the MPU9250 Lib cal for mag should be much better if it's working well.

I've used this "static cal" approach for my gyros and accel biases in my drone nav, and it's worked well, so curious to compare it to the Lib cals.

Don
 
Don: Did a change get made to skip integrating IMU data when (-1 == readSensor()) ? Maybe add a { qBlink(); delay(75); qBlink(); delay(50); qBlink(); delay(75); } in that case [with pinmode(13,OUTPUT); in setup] for visual trouble indicator.

Would be a shame to base calibration on unread/unchanging data if that is happening.
 
Tim,
I put in a simple check

dataQualityFlag = Imu.readSensor();
if (dataQualityFlag == -1){
Serial.println(">>>>>>>>>>>>> WARNING - BAD IMU DATA DETECTED <<<<<<<<<<<<<<<");
}

Right now I'm not seeing the bad hx numbers, but I think the circular buffer sometimes is not getting filled, even though I set the number of iterations higher than the buffer size. I'm trying different delays between readSensor commands, right now tried delay(20), which is pretty big, but got this:

Wait... Conducting IMU noise sigma calculation...
Static IMU Sensor Sigma Values:
axb Sigma (m/ss): 0.003302 axMax: 0.114928 axMin: 0.229855
ayb Sigma (m/ss): 0.005140 ayMax: 0.244221 ayMin: 0.229855
azb Sigma (m/ss): 0.004101 azMax: 0.000000 azMin: -10.396153
gxb Sigma (deg/s): 0.000349 gxMax: 0.000000 gxMin: -0.001233
gyb Sigma (deg/s): 0.000298 gyMax: 0.000000 gyMin: -0.001121
gzb Sigma (deg/s): 0.000466 gzMax: 0.000652 gzMin: -0.000546
hxb Sigma (uT): 0.823271 hxMax: 44.991642 hxMin: 42.441505
hyb Sigma (uT): 0.691879 hyMax: 22.040440 hyMin: 20.218916
hzb Sigma (uT): 0.401317 hzMax: 7.906937 hzMin: 6.852679

Note the three sets of zeros. Guessing this is buffer not getting filled. Was trying to keep this very simple without an IMU interrupt, but guessing I'll put the interrupt call in. What do you think?

EDIT: See next post. The max function appears to be returning 0.0000 is all numbers in buffer are negative.
 
Last edited:
tonton81,

Looks like I found an issue with the max calculation in circular buffer. If the numbers in the buffer are all negative (as if we're reading all -9.8 for gravity), the max function returns 0.0000 as the max instead of the negative number. So it looks like the max operation is expecting positive numbers.

See post above for an example.

I also did a test where I forced an array to all be about -10, and sure enough, the max came back as 0.0000 instead of -10.00.

Don
 
Cool, is there a special way to calculate it then? I could always correct that error

OH! I know what you mean now, ill figure it out tonight/tomorrow
 
Back
Top