uNav AHRS

Status
Not open for further replies.
Mike: Will wait for your next zip - I was also seeing INS9State in base name . . . only watching the debug window - not 6 versus 9 states so far ...

I made some minors edits - I'll redo them when I see that - still seeing lots more GPS misses than I saw before - even with 'printRate 100'

Did a quick exclusion of IMU processing when GPS_newData and that just makes output fitful - and still misses GPS?
 
Tim; Just sent an updated v2 in post 550? Did you see that one? I know I been missing posts in between typing them:)
 
< Yes - I saw that ... had to pause editing this - saw your #552 come in ... almost crossed again >

Mike: we cross posted. Finding the wrong INT number and restoring to my 50 turned back on my IMU updates.

Got the latest - will give it a go later - offline for now. If you see any issues or changes before then let me know.

I'm sort of wondering if there is some problem with memory or something messing with code flow? If somebody can do a sanity bounds check it might eliminate some mystery.
 
Don't know I am too close to this now. I think it flows right. Going to play with the threaded version and see what happens.
 
The way the GPS misses and the IMU over runs with no intervening Dtime() prints suggests something out of order. Perhaps there is misuse of one of the gating variables.

No doubt the time involved in the intense math in the neighborhood of 6ms delays GPS reporting - but the 100 byte message will overflow 64 byte default buffer if not caught . Maybe that is the fix - expand the PJRC default Rx buffer for the GPS port.

If the GPS Rx_isr() notes 'incoming message' and that fails - all processing will be delayed at least 200 ms until the next GPS report can complete - and if it is corrupted from the failed prior buffer remains - then it would take another full 200 ms cycle to clear it.

To correct on my Serial2 for GPS:
T:\arduino_1.8.5\hardware\teensy\avr\cores\teensy3\serial2.c line 43 changes to: #define SERIAL2_RX_BUFFER_SIZE 128 // 64 // number of incoming bytes to buffer

For Serial 1 and 3 the line # looks the same in: T:\arduino_1.8.5\hardware\teensy\avr\cores\teensy3\serial1.c and T:\arduino_1.8.5\hardware\teensy\avr\cores\teensy3\serial3.c
 
That worked! The attention to math'y stuff was allowing GPS buffer to suffer overrun - for reference here is my current - with the changes I made for what it shows:

View attachment MPU9250_INS9State_V5_X.zip

Output change is : ----- 11 ------XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX-------- #5 (! 11)

I accumulate all missing GPS Hz into what shows as '11' and "(! 11)" above where the "(! 11)" is always shown - the 11 are from startup

Also added is #466 - that is the count of received GPS updates - with ! 11 marked as missing:
#466 (! 11) @20:56.20_200355943 >> 2== ms_199

And minutes later:
#1221 (! 11) @20:58.51_200412122 >> 2== ms_200
 
I'll take a look at the changes and up my serial buffer. You know with all the posts talking about serial buffer changes wish there was a way to programmatically change the buffer. I'll have to look at it after this project.

EDIT: JUST took a look at your zipped file. Didn't see anywhere you made the change to output prints, ie.
Output change is : ----- 11 ------XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX-------- #5 (! 11)

I used code compare to check it. Also the sketch you posted is a little outdated so it was hard to track the changes. But I couldn't find it
 
Last edited:
(what I posted) Runs fine at 96 MHz now too. The 10 indicated failures came during the first 6 valid messages processed.
----- 10 ------XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX-------- #6 (! 10) @21:3.11_-491438 >> 1== ms_0

Please all - edit the appropriate Serial#.c file for Rx buffer size - it will need to be edited after EACH update to TeensyDuino as well due to overwrite.

Code:
-----		 #1981 (! 10)  @21:9.46_-344456 >> 1==	 ms_0
*^kfP__ uS=4827
kf_U_ uS=4356
^^GOT_IMU uS=10652
*^kfP__ uS=4803
^^^dtos_OutKF uS=710
GOT_IMU uS=7050
*^kfP__ uS=4814
^^^GOT_IMU uS=6216
*^kfP__ uS=4815
^^^GOT_IMU uS=6216
*^kfP__ uS=4788
^^^GOT_IMU uS=6184
*^kfP__ uS=4806
^^^GOT_IMU uS=6207
*^kfP__ uS=4817
^^^GOT_IMU uS=6214
*^kfP__ uS=4780
^^^GOT_IMU uS=6165
*^kfP__ uS=4812
^^^GOT_IMU uS=6204
*^kfP__ uS=4816
^^^GOT_IMU uS=6199
*^kfP__ uS=4814
^^^GOT_IMU uS=6206
*^kfP__ uS=4800
^^^GOT_IMU uS=6187
*^kfP__ uS=4808
^^^dtos_OutKF uS=729
GOT_IMU uS=7042
*^kfP__ uS=4820
^^^GOT_IMU uS=6208
*^kfP__ uS=4817
^^^GOT_IMU uS=6211
*^kfP__ uS=4817
^^^GOT_IMU uS=6202
*^kfP__ uS=4812
^^^GOT_IMU uS=6204
*^kfP__ uS=4810
^^^GOT_IMU uS=6196
*^kfP__ uS=4777
^^^GOT_IMU uS=6171
*^kfP__ uS=4810
^^^GOT_IMU uS=6197
*^kfP__ uS=4815
		 #1982 (! 10)  @21:9.46_199655618 >> 2==	 ms_205
 
I'll take a look at the changes and up my serial buffer. You know with all the posts talking about serial buffer changes wish there was a way to programmatically change the buffer. I'll have to look at it after this project.

FrankB be created an 'edit' for building to use DEFS.H that sits in local folder and with the build edits has that included to allow "#UNDEF and then #DEFINE" to override default settings. There is a thread for that - I did it once - but it needs to be redone with each TD install. He wrote a batch file w/diffs tool to perform the operation - not sure if that works as I only tried one many builds back and the file hacked may have evolved.

Update - 96 MHz good these minutes later after post #558:
#3663 (! 10) @21:15.22_399780887 >> 3== ms_398
 
By the way I edited my v2 that I posted as an updated with the serial1 rx buffer change. I removed all the extra readGPS's :) so far its running fine. Have to take just replace your Dtime routine. Maybe I missed something

EDIT: I will check out FrankB's post if I can find it :) I've done that before but not with serial but with other library functions. I have to remember how I did it.
 
Yes, that is the DEFS.h thread. Do a DIFF on the platform.txt to see what changed ... I never did that. Maybe the hack is easy enough?

Mike: Please stop posting while I'm typing :) . . . .

I've got your V2 - not unpacked yet. Post an update if desired - I'll be busy another couple hours + probably.

Not sure if Dtime() changed - put in another call " Dtime( 9, 0, 0 );" across all of IMU new data. I did update the debugGPS() for better simple tracking.

Been away and find the 96 MHz T_3.6 running here without errors after 10 at start:
#26120 (! 10) @22:30.13_800460510 >> 5== ms_799

Just rebuilt my last _X version for 168 MHz and printRate 25.
Not seeing issues yet after the start:
#662 (! 11) @22:33.18_399529411 >> 3== ms_399
 
Dtime( 9, 0, 0 );" - don't see it there is Dtime(10,0,0) around it now. I updated your debug gps routine but .... Anyway here is what I got, made it v3. Can we make any changes going forward using this version? The only states showing are 1-10-2-0 no 9. Mike

Oops here is the attachment:
 

Attachments

  • MPU9250_INS9State_V3.zip
    39.8 KB · Views: 85
Okay the baseline is now the _V3 code. Didn't get it yet. Does it have the GPS test in if((sincePrint > printRate)) - I just saw my _X version does not? It was this to get GPS and delay a print if GPS available: if((0==GPS_newData) && (sincePrint > printRate)){

My _X should have had the #9 Above #10 to the end of the IMU processing right after the if(sincePrint)? No biggie.

The Dtime() is very clean to use and handy - no local test and setting or variables needed or ugly added .print for output and clean search for and to add or remove - glad I thought of it - very handy to see and quantify the hotspots in the code! Since #'s are independent one can include others without trouble. Could even add another to show and mark arrival of each IMU data set.

Doing this on #8 would track IMU data processing time - would be VERY noisy without special handling in Dtime like #10 gets ( just print '.' unless the time is longer than expected for the update rate "Imu.setSrd(9);") - some IMU data will be missed with the length of time the Filter math takes! In fact the _isr() will show a fresh time tag when the data is likely to get stale!:
Code:
Dtime( 10, 0, 0 );
Dtime( 8, 1, "nextIMU" );
    getIMU();
Dtime( 8, 0, 0 );
Dtime( 10, 10, "getIMU" );

The above made me think the setSRD should come from a #define in A_ConfigDefines - then the expected update rate would be known at compile time.

Also reminds me that setSRD() doesn't work as expected! On my machine it needs to be called again after: Imu.calibrateGyro();

You asked about 'doing readGPS()' in the _isr : Answer should be No. As a general rule - any isr() needs to be clean and minimal without any calls to complex code like reading uBlox.parse or serial input that can consume time or play with hardware or interrupts or generate any Serial output.
 
Yep - it has the correct GPS tests incorporated.

My _X should have had the #9 Above #10: nope - didn't see it.

I make the wrapper to imu - match yours.

Also reminds me that setSRD() doesn't work as expected! - forgot about that one.

Calling GPS_read from the isr - tried it - didn't work. But thanks.
 
Yep - it has the correct GPS tests incorporated.

My _X should have had the #9 Above #10: nope - didn't see it.

I make the wrapper to imu - match yours.

Also reminds me that setSRD() doesn't work as expected! - forgot about that one.

Calling GPS_read from the isr - tried it - didn't work. But thanks.

No worry - it is working well at 168 MHz now showing: ----- #47695 (! 11) @1:10.5_56740 >> 1== ms_0

The other Dtime stuff is something I can do it it shows anything.

That was : No - as a general rule - complex code in isr() - it should not be tried. Glad it failed on you to show how evil it is.

Just back home from errands - now need to go put fresh headlights in the Jeep.
 
As Tim said. Looks like in terms of timing doesn't seem to have any problem. The only issue seem to be in v3 is the kfUpdate. The data is wrong. If I run the same code as threaded it looks better. Here is a threaded version. No I am not enamored with threads but its a good learning experience.
 

Attachments

  • MPU9250_FiltersGPS_V6_threaded.zip
    40.3 KB · Views: 81
Given post about timing - recompiled the _X version I had now at 48 MHz and running fine without fail on GPS - after the first 13 clearing the buffer and starting:

#9729 (! 13) @4:42.58_199847144 >> 2== ms_201

Will open the _V3 now. ... it compiled at 48 MHz too and after initial start errors the GPS is coming in fine.

#2729 (! 10) @5:1.22_600249521 >> 4== ms_601
 
Last edited:
I dropped in my updated debugGPS to the _V3 code. Other minor tweeks to visualize/track internal timing.

Here is what I have as updated - notes below: View attachment MPU9250_INS9State_V3 (2).zip

Here are the times at 48 MHz - they are much longer than below at 240 MHz - but it still seems to have 20 getIMU's between GPS msg's on both - they just take ~100 uS longer to process each one.
Code:
-----		 #8371 (! 10)  @5:20.11_-338892 >> 1==	 ms_0
getIMU uS=846
^kfP__ uS=4120
kf_U_ uS=8681
^ecef2lla uS=940
getIMU uS=846
^kfP__ uS=4100
^^ecef2lla uS=874
getIMU uS=845
^kfP__ uS=4110
^^ecef2lla uS=880
dtos_OutKF uS=1969
getIMU uS=842
^kfP__ uS=4117
^^ecef2lla uS=945
getIMU uS=845
^kfP__ uS=4120
^^ecef2lla uS=939
getIMU uS=850
^kfP__ uS=4118
^^ecef2lla uS=935
dtos_OutKF uS=1797
getIMU uS=844
^kfP__ uS=4116
^^ecef2lla uS=938
getIMU uS=839
^kfP__ uS=4114
^^ecef2lla uS=944
getIMU uS=850
^kfP__ uS=4114
^^ecef2lla uS=942
dtos_OutKF uS=1799
getIMU uS=847
^kfP__ uS=4115
^^ecef2lla uS=939
getIMU uS=847
^kfP__ uS=4112
^^ecef2lla uS=943
getIMU uS=846
^kfP__ uS=4117
^^ecef2lla uS=939
dtos_OutKF uS=1938
getIMU uS=846
^kfP__ uS=4113
^^ecef2lla uS=937
getIMU uS=846
^kfP__ uS=4109
^^ecef2lla uS=942
getIMU uS=846
^kfP__ uS=4112
^^ecef2lla uS=945
dtos_OutKF uS=1917
getIMU uS=845
^kfP__ uS=4109
^^ecef2lla uS=943
getIMU uS=851
^kfP__ uS=4116
^^ecef2lla uS=940
getIMU uS=843
^kfP__ uS=4111
^^ecef2lla uS=937
dtos_OutKF uS=1937
getIMU uS=850
^kfP__ uS=4118
^^ecef2lla uS=941
getIMU uS=851
^kfP__ uS=4201
^^ecef2lla uS=943

Then recompiled at 240 MHz and debug streams much better as output is more compact with the getIMU is recorded only as '*' being under 750 uS. I made the Dtime(x,10,dtos_OutKF) to limit printing if under 750uS to just '^'.

I also added the inverse timing to show "SlowIMU" when the data is not read on setSRD() schedule with this and added IMU_SRD to the A_Config file. This triggers rarely at 240, a bit more at 120 MHz only after GPS completion report:
>> if ( Dtime( 12, 2, 0 ) > (1000000/(1000/(IMU_SRD+1)) ) ) Dtime( 12, 1, "SlowIMU" );

It compiles and runs normally when "coutD" not defined. To do that I have to add ifdef to "void debugGPS1()" in serialPort.ino - that can be removed now?

This is the current cryptic output showing all is well - with SlowIMU note where expected:
Code:
^^^		 #4790 (! 9)  @7:54.59_600072965 >> 4==	 ms_599
SlowIMU uS=21272
*^kfP__ uS=834
kf_U_ uS=1771
^^^*^kfP__ uS=832
^^^*^kfP__ uS=835
^^^^*^kfP__ uS=834
^^^*^kfP__ uS=833
^^^^*^kfP__ uS=835
^^^*^kfP__ uS=834
^^^^*^kfP__ uS=834
^^^*^kfP__ uS=833
^^^^*^kfP__ uS=834
^^^*^kfP__ uS=849
^^^^		 #4791 (! 9)  @7:54.59_800073039 >> 5==	 ms_799
 
Nice - just loaded it up and have it running now. Agreed - don't need the debugGPS1 now - left it there - just in case :) Did leave in the other serial prints though. There are other filters to deal with. Oh, here is a typical out from the T3.5:

Code:
		 #2042 (! 13)  @13:8.26_800276265 >> 5==	 ms_779
*^kfP__ uS=2450
kf_U_ uS=5048
^^*^kfP__ uS=2444
^^^*^kfP__ uS=2453
^^^dtos_OutKF uS=822
*^kfP__ uS=2456
^^^*^kfP__ uS=2456
^^^*^kfP__ uS=2455
^^^dtos_OutKF uS=819
*^kfP__ uS=2456
^^^*^kfP__ uS=2453
^^^*^kfP__ uS=2454
^^^dtos_OutKF uS=822
*^kfP__ uS=2454
^^^*^kfP__ uS=2455
^^^*^kfP__ uS=2454
^^^dtos_OutKF uS=840
*^kfP__ uS=2452
^^^*^kfP__ uS=2455
^^^*^kfP__ uS=2453
^^^dtos_OutKF uS=835
*^kfP__ uS=2453
^^^*^kfP__ uS=2452
^^^*^kfP__ uS=2453
^^^dtos_OutKF uS=836
*^kfP__ uS=2452
^^^*^kfP__ uS=2483
^^^
-----		 #2043 (! 13)  @13:8.27_276479 >> 1==	 ms_0
 
Ran a little longer test with SRD at 9 (100 hz):
#10717 (! 18) @13:58.22_800473041 >> 5== ms_787

but the data looks strange for qModel = 1:

Code:
 utcText, tsIMUText, tsGPSText, latText, lonText, altText, pk1xText, pk1yText, pk1zText, nuk1xText, nuk1yText, nuk1zText
50406.199219, 2248.1904, 2248.1680,-79.720062,  0.784161,260127024.0000,850336.6875,850336.6875,850336.6875,-15328811008.0000,-2332258048.0000,62337638400.0000

Typical output from the threaded version would be (qModel=2):
Code:
50754.199219,    4.6277,    4.6146, 40.774689,-73.814339,    8.6888,    1.0720,    1.0720,    1.0720,  -54.9607,  -14.6054,   26.0399

the equivalent for the non-threaded version:
Code:
51049.601562,   32.9222,   32.8084,-10.870099,-13.635868,3709600.2500,18120.2676,18120.2676,18120.2676,-40006260.0000,-11118527.0000,29454056.0000

On rtk.upDated in the getGPS function the data from the filter would look like (based on serialDataOut call) with the KF output on GPS read:
Code:
  0.004000, 51263.199219,  19.166672, 39.823009, 3 40.77489778,-73.81468626,   48.8620    0.0320,    0.0100,   -0.0250  -49.0330,   40.9361, -132.1514    1.1349,   -7.9872,    4.1521
51263.199219,   19.1767,   19.1646, 40.774895,-73.814674,   49.3100,    5.0000,    5.0000,    5.0000,17910428.0000,-6830290.0000,3965363.7500

Not sure whats going on.
 
A couple of ideas...
- As a baseline, we might check any version at the 5Hz GPS rate just to make sure the KF routines are doing their stuff. This means that kfProp and kfUpdate would both be run at 5 Hz. The innovations (residuals) are the last three terms in the serial output, and they should be on the order of 1.0 or so.
- If that works, it still doesn't mean that the IMU accel values aren't causing issues in kfProp. So then run kfProp at the faster IMU update rates but WITHOUT accel being added (comment out the "+ gMatrix * accelNED" part of the xk1Minus calculation).
- If the results still look good, then we add back in the "+ gMatrix * accelNED" part of the xk1Minus calculation. If this makes the innovations (aka as the residuals, the last three terms in the serial print line) jump, then accelNED is probably the culprit. My guess is that at faster kfProp rates, accelNED may be causing issues if we don't smooth it.

I'll try these steps out if you'd like. If so, what version would you like me to work with?

P.S. On my RTK drone project I found one issue today (may not be the only one...). The GPS connector on my PixHawk2.1 was loose, and finally fell off today. So I think it's been only intermittently working and thus the issues with loading GPS firmware and getting an RTK lock. Aaaarrrggg. The soldering on the PixHawk2.1 looks pretty poor on the connectors.
 
Brian-Don-Tim
The stubborn person I am I went back to the uNavAHRS code to try and see what's going on. I made two changes to the library:
1. I forced the magUpdate to be true all the time, like this:
Code:
  // checking if magnetometer data updated
  if ((hx!=hx_)||(hy!=hy_)||(hz!=hz_)) {
    magUpdated_ = true;
    hx_ = hx;
    hy_ = hy;
    hz_ = hz;
  } else {
    magUpdated_ = true;
  }
2. I moved _dt to before the test for gyroUpdated:
Code:
  } else {
    /* time update */
      // get the change in time
      _dt = (float)_t /1000000.0;
      _t = 0;
    if (gyroUpdated_) {

I also had the temperature printed out to the TViewer to see if there was any correlation to changes in yaw drift. I think there is based on what I saw. I will post some graphs later. I doing a little research I saw that the MPU-9250 does have the ability to do temperature compensation for the accelerometer at least. I found that in the spec and in Kris Winer's MPU-9250 example for his board. Don't think that the MPU-9250 library currently does that - saying think because I haven't really looked yet. I am attaching my sketch and the TViewer file in case you all want to give it a test run.

Cheers
Mike

EDIT: Found a link you all might be interested in, nice reference, http://pataga.net/imu_kalman_filter_notes.pdf
 

Attachments

  • uNavAHRSv1_MPU9250.zip
    12.4 KB · Views: 83
  • kf7e.txt
    5.1 KB · Views: 83
Last edited:
Mike or Tim,
With GPS reading at 5Hz, about what speed were you able to read the IMU data?

Looks like I need to move the kfProp call to before the "if (0==GPS_newData)" line so the KF can use dt_ to propagate the state.
 
Status
Not open for further replies.
Back
Top