uNav AHRS

[defragster]

I tried to put readGPS(); into yield() and that was a BIG FAIL to run - probably called before setup() of GPS is complete?

Too many readGPS(); calls are present - but I uploaded it that way anyhow < updated output from that shown below >
... re-updated see post #528 ...


I set the IMU 'now' time in the _isr when the interrupt says the data is ready. I also bumped up my IMU i2c Clock speed taking the transfer portion of those calcs down a bit. Since we are using wire not i2c_t3 code I can't see what the net clock speed in use is but I asked for 4 MHz.

The trend of uninterrupted IMU reads continues and goes along with the failure? I put a Dtime() in the print routine - and during this period no prints are made and no GPS or other normal calculations are made.

This points to printRate :: so I set that up from 25 to 50. Something goes wrong when this runs too often? :: if((0==GPS_newData) && (sincePrint > printRate)){
>> There is something odd here as they come in a group with no other debug output that should occur in the normal course of events??

I am now seeing all 5 Hz GPS updates at 240 MHz AND 168 MHz. I am sync'ing the debugGPS() on the GPS seconds to count and separate them versus an internal clock.

I left all my debug edits inline - if compiled with undefined "#define coutD" - it compiles without error and while calls remain the code safely goes away.

Here is a second plus either side of those 5 updates at 240 MHz - some measured times under 700 uS are not shown::

		 @7:47.58 >> 5==	 ms_800
getIMU uS=612
getIMU uS=609
getIMU uS=610
kfP__ uS=1955
kf_U_ uS=1762
dtos_OutKF uS=230
getIMU uS=616
getIMU uS=610
getIMU uS=608
getIMU uS=608
getIMU uS=608
getIMU uS=609
kfP__ uS=1954
kf_U_ uS=1763
dtos_OutKF uS=232
getIMU uS=616
getIMU uS=611
getIMU uS=609
getIMU uS=609
getIMU uS=608
getIMU uS=608
kfP__ uS=1958
kf_U_ uS=1763
dtos_OutKF uS=233
getIMU uS=617
getIMU uS=611
getIMU uS=607
getIMU uS=609
getIMU uS=609

-----		 @7:47.59 >> 1==	 ms_0
getIMU uS=613
kfP__ uS=1951
kf_U_ uS=1764
dtos_OutKF uS=212
getIMU uS=616
getIMU uS=610
getIMU uS=609
getIMU uS=609
getIMU uS=609
getIMU uS=608
kfP__ uS=1955
kf_U_ uS=1763
dtos_OutKF uS=213
getIMU uS=615
getIMU uS=611
getIMU uS=609
getIMU uS=608
getIMU uS=609
getIMU uS=608
kfP__ uS=1955
kf_U_ uS=1769
dtos_OutKF uS=206
getIMU uS=617
getIMU uS=609
getIMU uS=610
getIMU uS=608
getIMU uS=608
getIMU uS=608
kfP__ uS=1955
kf_U_ uS=1764
dtos_OutKF uS=211
getIMU uS=615
		 @7:47.59 >> 2==	 ms_199
getIMU uS=614
getIMU uS=610
getIMU uS=609
getIMU uS=607
getIMU uS=608
kfP__ uS=1955
kf_U_ uS=1762
dtos_OutKF uS=213
getIMU uS=616
getIMU uS=610
getIMU uS=609
getIMU uS=608
getIMU uS=608
getIMU uS=608
kfP__ uS=1950
kf_U_ uS=1764
dtos_OutKF uS=208
getIMU uS=617
getIMU uS=610
getIMU uS=609
getIMU uS=608
getIMU uS=609
getIMU uS=608
kfP__ uS=1953
kf_U_ uS=1765
dtos_OutKF uS=211
getIMU uS=616
getIMU uS=611
getIMU uS=610
		 @7:47.59 >> 3==	 ms_400
getIMU uS=611
getIMU uS=610
getIMU uS=609
kfP__ uS=1955
kf_U_ uS=1764
dtos_OutKF uS=225
getIMU uS=617
getIMU uS=611
getIMU uS=608
getIMU uS=608
getIMU uS=609
getIMU uS=608
kfP__ uS=1953
kf_U_ uS=1763
dtos_OutKF uS=231
getIMU uS=615
getIMU uS=610
getIMU uS=608
getIMU uS=607
getIMU uS=608
getIMU uS=607
kfP__ uS=1954
kf_U_ uS=1763
dtos_OutKF uS=227
getIMU uS=616
getIMU uS=610
getIMU uS=609
getIMU uS=609
getIMU uS=610
		 @7:47.59 >> 4==	 ms_600
getIMU uS=613
kfP__ uS=1950
kf_U_ uS=1764
dtos_OutKF uS=230
getIMU uS=616
getIMU uS=610
getIMU uS=609
getIMU uS=608
getIMU uS=607
getIMU uS=608
kfP__ uS=1956
kf_U_ uS=1759
dtos_OutKF uS=230
getIMU uS=617
getIMU uS=610
getIMU uS=608
getIMU uS=608
getIMU uS=608
getIMU uS=608
kfP__ uS=1956
kf_U_ uS=1759
dtos_OutKF uS=231
getIMU uS=616
getIMU uS=610
getIMU uS=609
getIMU uS=608
getIMU uS=609
getIMU uS=609
kfP__ uS=1956
kf_U_ uS=1763
dtos_OutKF uS=233
getIMU uS=616
		 @7:47.59 >> 5==	 ms_800
getIMU uS=614
getIMU uS=610
getIMU uS=608
getIMU uS=609
getIMU uS=608
kfP__ uS=1954
kf_U_ uS=1763
dtos_OutKF uS=231
getIMU uS=616
getIMU uS=611
getIMU uS=609
getIMU uS=609
getIMU uS=608
getIMU uS=608
kfP__ uS=1948
kf_U_ uS=1762
dtos_OutKF uS=229
getIMU uS=616
getIMU uS=611
getIMU uS=609
getIMU uS=608
getIMU uS=608
getIMU uS=608
kfP__ uS=1954
kf_U_ uS=1761
dtos_OutKF uS=233
getIMU uS=616
getIMU uS=609
getIMU uS=608
getIMU uS=622

-----		 @7:48.0 >> 1==	 ms_0
getIMU uS=611
getIMU uS=609
kfP__ uS=1949
kf_U_ uS=1762
dtos_OutKF uS=213
getIMU uS=615
getIMU uS=611
getIMU uS=607
getIMU uS=609
getIMU uS=609
getIMU uS=608
kfP__ uS=1952
kf_U_ uS=1761
dtos_OutKF uS=211
getIMU uS=617
getIMU uS=611
getIMU uS=611
getIMU uS=608
getIMU uS=609
getIMU uS=608
kfP__ uS=1955
kf_U_ uS=1762
dtos_OutKF uS=210
getIMU uS=615
getIMU uS=610
getIMU uS=610
getIMU uS=609
getIMU uS=609
getIMU uS=621
		 @7:48.0 >> 2==	 ms_200

<EDIT>: Updated posted code - getIMU noise prints are now just a '^' each - as are all other Dtime(x,10,"") events under 750 uS. Will update sample output in following post.

 

[defragster]

For Kicks I recompiled at 120 MHz and it works 5 Hz GPS without fail. You can see a fail go by with a longer bar of '---------------------------'

Mike: I'm assuming you will MERGE or update this to V6? It should work on the T_3.5 at 120 MHz for you. Then perhaps start pulling out some of the excess ~10 readGPS(); and see if that makes it fail. <edit>The Dtime() pair around getIMU(); was changed to reduce noise - but that '^' repetition shows trouble that needs to be found before a GPS miss. Other Dtime(x,10,"") that do not print long times can be removed.

Perhaps you will be able to tell where the collapse happens for printing too fast?

BTW: If you haven't noticed @tonton81 on his SPI thread may workout an SPI MASTER<>SLAVE so a second Teensy could be passed a binary data block to feed TViewer. The goal is to have the SLAVE get the raw data block at SPI speeds, then as a dedicated processor convert and format the stored data to the float CSV string that TViewer wants. Does everyone have a spare 2nd Teensy to wire SPI to the PRIMARY unit (on a spare SPI port) { replacing the debug unit perhaps being used for DEBUG OUTPUT } ? That 2ND teensy would talk exclusively to TViewer over USB. Allowing the PRIMARY unit to be programmed normally and use Serial Monitor on USB for debug. Those SPI messages would transmit less data (<50 bytes) faster (perhaps 25 MHz) and offload the 200 to 400 uS used to do the string formatting conversion for each print as well as the USB printing of the 110+ bytes. So TViewer data set could be expanded as needed and updated faster to get clearer results - without impacting the CORE FILTER code so significantly.
NOTE: tonton81 also has commandeered ESP8266 units for WiFi message passing - so if this works it could evolve to wireless second Teensy ( adding WiFi ESP8266 to replace SPI ) - that would be Tether free TViewer monitoring.

Here is one GPS report period at 120 MHz with updated ZIP(3) above. IMU and other events under 750uS are just a '^' so you know they happened ( should have added an ID indicator ).
It shows below where no updates except "^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^" appear and then the 4th "----- 4 --" missed GPS event occurred with added counter.
Also the digital formatted timecode now includes the 'nano' gps element >@9:0.23_600270055 :

-----		 @9:0.23_269830 >> 1==	 ms_0
^^^kfP__ uS=3872
kf_U_ uS=3479
^^dtos_OutKF uS=399
^^^^^^^^kfP__ uS=3871
kf_U_ uS=3480
^^dtos_OutKF uS=398
^^^^^^^^kfP__ uS=3872
kf_U_ uS=3482
^^dtos_OutKF uS=397
^^^^^^^^kfP__ uS=3872
kf_U_ uS=3529
^^dtos_OutKF uS=398
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^		 @9:0.23_600270055 >> 2==	 ms_597
^^^kfP__ uS=3871
kf_U_ uS=3480
^^dtos_OutKF uS=445
^^^^^^^^kfP__ uS=3868
kf_U_ uS=3485
^^dtos_OutKF uS=441
^^^^^^^^kfP__ uS=3870
kf_U_ uS=3485
^^dtos_OutKF uS=439
^^^^^^^^kfP__ uS=3873
kf_U_ uS=3483
^^dtos_OutKF uS=439
^		 @9:0.23_800270131 >> 3==	 ms_797
^^^^^^^kfP__ uS=3869
kf_U_ uS=3482
^^dtos_OutKF uS=407
^^^^^^^^kfP__ uS=3873
kf_U_ uS=3481
^^dtos_OutKF uS=407
^^^^^^^^kfP__ uS=3872
kf_U_ uS=3483
^^dtos_OutKF uS=408
^^^^
-----	4  -------------------------------------------		 @9:0.24_270206 >> 1==	 ms_0
^^^^kfP__ uS=3868
kf_U_ uS=3481
^^dtos_OutKF uS=404
^^^^^^^^kfP__ uS=3872
kf_U_ uS=3478
^^dtos_OutKF uS=398
^^^^^^^^kfP__ uS=3871
kf_U_ uS=3481
^^dtos_OutKF uS=410
^^^^^^		 @9:0.24_200270281 >> 2==	 ms_200

This shows the 7th miss detected about 8.5 minutes later and then the 9th - much later ... #40.

----- 7 ------------------------------------------- @9:9.0_464310 >> 1== ms_0
----- 9 ------------------------------------------- @9:15.1_-399978 >> 1== ms_0

----- 40 ------------------------------------------- @10:12.36_-111984 >> 1== ms_0

 

[defragster]

MPU9250_INS9State_V5 (4)

Debug a bit more telling - nothing fixed {except a forbidden char*} - and 4 hits on GPS looked like 5:: IMU Dtime(10,10,"") will print '*' versus '^'.

Bumped to 168 MHz and not showing fails - so timing is on the edge with output rate/quantity.

View attachment MPU9250_INS9State_V5 (4).zip

Mostly no fails in 12 mins until this real oddity - 5 clean seconds of GPS - with no IMU?

<first three on startup are catch on GPS overlapping> missed #4 and #5 reported below.

>> Almost seems like there was a loop somewhere running nothing but readGPS() for 5 seconds?
>> Something similar would explain the IMU spasms ( like '****************************************' ) - except I only see that call once in loop()?
Is the code not being stable?

-----		 @10:33.34_342508 >> 1==	 ms_0
*****^^kfP__ uS=2776
kf_U_ uS=2497
^^dtos_OutKF uS=300
******^^kfP__ uS=2768
kf_U_ uS=2497
^^dtos_OutKF uS=294
******^^kfP__ uS=2774
kf_U_ uS=2496
^^dtos_OutKF uS=296
***		 @10:33.34_200342626 >> 2==	 ms_195
		 @10:33.34_400342697 >> 3==	 ms_395
		 @10:33.34_600342768 >> 4==	 ms_597
		 @10:33.34_800342840 >> 5==	 ms_799

-----		 @10:33.35_342911 >> 1==	 ms_0
		 @10:33.35_200342982 >> 2==	 ms_199
		 @10:33.35_400343053 >> 3==	 ms_399
		 @10:33.35_600343125 >> 4==	 ms_602
		 @10:33.35_800343196 >> 5==	 ms_800

-----		 @10:33.36_343268 >> 1==	 ms_0
		 @10:33.36_200343339 >> 2==	 ms_198
		 @10:33.36_400343411 >> 3==	 ms_396
		 @10:33.36_600343482 >> 4==	 ms_597
		 @10:33.36_800343554 >> 5==	 ms_799

-----		 @10:33.37_343626 >> 1==	 ms_0
		 @10:33.37_200343697 >> 2==	 ms_200
		 @10:33.37_400343769 >> 3==	 ms_400
		 @10:33.37_600343840 >> 4==	 ms_598
		 @10:33.37_800343912 >> 5==	 ms_799

-----		 @10:33.38_343983 >> 1==	 ms_0
		 @10:33.38_200344055 >> 2==	 ms_199
		 @10:33.38_400344125 >> 3==	 ms_399
		 @10:33.38_600344195 >> 4==	 ms_600
		 @10:33.38_800344265 >> 5==	 ms_801

-----		 @10:33.39_344324 >> 1==	 ms_0
*^^kfP__ uS=2735
kf_U_ uS=2502
^^dtos_OutKF uS=311
******^^kfP__ uS=2777
kf_U_ uS=2494
^^dtos_OutKF uS=300
******^^kfP__ uS=2775
kf_U_ uS=2495
^^dtos_OutKF uS=302
******^^kfP__ uS=2768
kf_U_ uS=2499
^^dtos_OutKF uS=300
**		 @10:33.39_200344395 >> 2==	 ms_202
****^^kfP__ uS=2777
kf_U_ uS=2494
^^dtos_OutKF uS=299
******^^kfP__ uS=2773
kf_U_ uS=2494
^^dtos_OutKF uS=292
******^^kfP__ uS=2769
kf_U_ uS=2495
^^dtos_OutKF uS=299
****		 @10:33.39_400344466 >> 3==	 ms_402
**^^kfP__ uS=2779
kf_U_ uS=2494
^^dtos_OutKF uS=298
******^^kfP__ uS=2779
kf_U_ uS=2495
^^dtos_OutKF uS=298
******^^kfP__ uS=2770
kf_U_ uS=2495
^^dtos_OutKF uS=292
******^^kfP__ uS=2778
kf_U_ uS=2521
^^dtos_OutKF uS=300
****************************************
-----	5  -------------------------------------------		 @10:33.40_344679 >> 1==	 ms_0

 

[mjs513]

Tim you been busy.... going to take awhile to absorb everything here. But in answer to your question - yes I merge into a an updated version later today. I did some more cleanup last night and pre-9state version. Will keep you posted during the course of the day.

 

[mjs513]

Ok. Had a little coffee in me now so here goes some answers.

Don: " noticed that it's printing 5 times for each 5Hz time-stamp" - don't worry - that's on purpose and easy to change. The first is the updated gps with what was imu data now kf data the other 4 times was just for reference showing not gps data changed in the sinceprints loop.

Tim: Still going through your code and merging the debug in my reworked code.. If you look, readGPS is called multiple times in the loop - was trying to figure out where it was failing = there was also a stray readGPS in the SerialPrintKF code.

v4 was a real experimental release trying to get it debugged. In the reincarnation I was working on last night for the 6state was putting the whole GPS code that's in the loop into a separate function call and doing a bunch of clean up. Then calling that function and slowly removing my getGPS calls that I have scattered in the imu loop to see where it fails.

Right now the working version I have seems to be doing ok looking at your debug calls which are a big help. Been staring at the screen for the last 5 minutes and not seeing any issues. I probably should at this point start looking at removing extra getGPS to see where it starts failing. I am posting my updated 6state code for reference as version 4a. Going to start merging the 9state a little later. You might want to version 4a a try on the t3.6 just in case.

 

[mjs513]

Things are changing fast. Was real easy to incorporate the 9-state into the 6-state version . However, while it looks like we got the 6-state working the 9-state is failing. When I say failing, its loosing GPS updates in the kfProp function with the addition of the calculateQ function. Thanks Tim for the debugGPS routine, works really nice.

The other changes I had made to print statement was the addition of the IMU and GPS timestamps which are based off the interrupts. Using that as a gauge they are pretty close.

Tim: If you put the getGPS call in the interrupt would it work?

Anyway. Here is a new baseline version for the 9-state. Want to do some config control, this way we are not mixing apples and oranges.

Mike

 

[Don Kelly]

Tim,
The PCB route looks interesting. I'm also thinking of eventually trying Brian's Marmot board, has nice interfaces but doesn't come with GPS. And not sure how precise IMU timing would be done with his board, whether he's envisioning leveraging PPS or what. I can also see myself eventually going to the uBlox m8p to get the rtk interface. I'm experimenting with them on my drone now (via the HERE+ RTK kit).

Don

 

[Don Kelly]

Mike,
The calculateQ offers 3 different models for calculating qMatrix, two are time (_dt) dependent and one (Model 1) just contains fixed values. So for testing purposes, we could use Model 1 and call calculateQ in the kfInit routine rather than kfProp as an option.

Once we have kinks and timing worked out, we would want to come back and optimize our settings for the Q and R matrices (the "tuning" parameters in a Kalman). But for now it's probably adequate to use the Model 1 (fixed Q) setting and not call it for each kfProp.

 

[Don Kelly]

As far as config control, I need to get onboard with using GitHub I suppose! I've set up an account but haven't configured my PC yet.

 

[Don Kelly]

Mike,
Taking a quick look at the serial output for your last release (V1), it looks like the Kalman may not be working right. The covariance (pk1) and innovations (nuk1) should be on the order of a meter (~1.) or so, and they're huge, so something weird going on i think.

kfProp can be called once for every time there's new IMU data with a new time stamp. kfUpdate can be called once for every time there's new GPS data and time stamp. But in the serial printout, I'm seeing nuk1 (which is calculated in kfUpdate) with multiple values associated with repeated utc time stamps, so it seems like kfUpdate may be getting called multiple times for each GPS measurement / time stamp. I'll go back and try to figure out the mods made in this version, but printout results seem off unless I'm not understanding something (which is a high possibility!).

 

[mjs513]

Hi Don,

First I was going to ask you about whether the data was any good. It didn't look right to me. If you take out the rtk.updated test before the kfUpdate it works. Wanted to do a little more debugging on that one. kfProp should only update when imu data is updated since it is in the loop. Check the timing tests you should see how often they are updating. By the way saw this with the 6 state also when I kfUpdate is not being updated each imu update. When kfUpdate is called you can see that pk1 and nuk1 are all updated correctly. Yes I have been staring at the data a long time.

The calculateQ offers 3 different models for calculating qMatrix - thanks, didn't even notice. That should help with the debugging.

Mike

 

[Don Kelly]

tried commenting out the rtk.updated test, but still not working. so I'll print out the main loop and sit down with cup of coffee and go thru in more detail...

 

[mjs513]

Funny, it works if you do that in the 6-state version. Unfortunately, when you do that it misses more gps updates.

 

[brtaylor]

Just arrived back in Houston, ready to re-engage. Was on family trip, so pretty much only looked at code on the plane. Got kfInit debugged, have kfProp and kfUpdate to go. Then I need to catch up on what you two have been up to, sounds like some cool stuff over the last 4-5 days!

Once I get the kf routines debugged, I'll need to think about best ways to smooth out the accel data that comes out of the AHRS. I'm guessing we'll be able to get accel (body frame) from the AHRS at a fairly good clip, and then am hoping we can smooth it down to some slower rate and do the conversion to accel (NED-inertial). This (accel NED-inertial) is then what goes into the kfProp step, hopefully not so noisy. Then the kfUpdate step blends in the GPS position and velocity measurements. kfProp would be nice at something like 20Hz, whereas kfUpdate would run at the GPS rate (like 5Hz). Anyway, these are my thoughts so far.

Brian, you were running the propagate and update steps at same rate of the GPS receiver (5Hz) I believe in your implementation. What is your current thinking? For something like high accuracy drones applications, I'm hearing that folks are working towards wanting the INS/GPS to output on the order of 20Hz, so that's why I'm thinking that running the kfProp at 20 Hz (or faster, depending on the quality of accel control inputs) and the kfUpdate at 5Hz might be cool.

I'm finally starting to read back through the thread, but might not fully get back to developing until the second week of March, once all proposals are submitted (can't wait!).

I would like to run the time update at the IMU rate, and I think that's what the uNavINS code is doing. Measurement update occurs at the GPS rate.

 

[brtaylor]

Mike - what issues are you seeing that is driving you nuts? Slowdowns from output again? Or lag from the filters running too long?

Is there any way the filters could be made interruptible? Where they could process a finite part or portion exit and return where they left off? That is what TThreads provides.

Will look at V5_Threaded ...

Indeed 30 ga heats in solder fast and can burn quickly depending on coating - but tape or the coating can keep it safe.

Seemed Brian was looking for uBlox to have MAG data with it - was surprised it was separate i2c bus and not integrated when I got mine. Wasn't sure if it added value and would get used.

Not really looking for mag data from the GPS. Could be nice, since it could be further away from some electronics and provide a better source of heading data, but just another sensor to write drivers for and figure out how to integrate. Mostly, I picked up the hobby king uBlox because it used to be very inexpensive compared to other 8 series uBlox modules I could find. Now I'm recommending the mRobotics breakouts because they're using the JST-GH connectors, which have become almost standard for UAVs.

 

[brtaylor]

Brian...

Curious to hear your thoughts on the "PPS for timing" topic. Do you have any ideas on how to get accurate IMU timing other than syncing to PPS?

Not yet. I haven't looked yet at the IMU timing compared to a "true" source like GPS time to see how bad the IMU timing drifts. For the INS, we're more concerned about the delay between the IMU and GPS. Right now I ignore it, but the correct approach would be to:
1. Do a time update at the IMU rate, buffer the IMU data
2. When a new GPS measurement comes in, do a measurement update at the corresponding IMU data (this will be back in the buffer, rather than at the head, due to delay).
3. Re-apply the time update using buffered IMU data from the point the measurement update was done up to the current sample

 

[brtaylor]

Just a quick update on my end. I'll be mostly a ghost until the second week in March. One of my colleagues modified our original 15 state EKF to use floats wherever possible, which is basically everything other than position. We're running the EKF on a BeagleBone Black and saw a good speedup without a loss in accuracy. He's doing extensive simulation work with his modified filter to compare its performance against our original filter. The modified version will likely become the default used in the University of Minnesota UAV Lab. Assuming that my colleague doesn't find any errors with his port, I'll probably make the few minor changes needed to run on an Arduino and see how that compares to my earlier port, uNavINS. Hopefully this will help sort out some of the bugs I was encountering with the uNavINS.

Excited to get back into all of this development and see if we can get a set of good filters to run on Arduino.

 

[mjs513]

Thanks for the update Brian. Good luck with your grant writing .

 

[Don Kelly]

Mike, Tim,
What are your thoughts on Brian's post above on his suggested approach?
1. Do a time update (i.e., kfProp) at IMU rate and buffer
2. When new GPS meas arrives do meas update (i.e., kfUpdate) for appropriate IMU reading
3. Re-apply time update (i.e., kfProp) from GPS meas time to current time

I'd need to make sure kfReset is run at the right time with the right flags, but seems pretty straight-forward.

My idea was a bit different (create a RTC via GPS PPS) but Brian's looks easier, I think...

 

[defragster]

Brian-
Check: Mag on GPS is just there - My $20 unit has one to ignore. It also has JST connector - and per my pics - also through pin holes. I socketed so I can use either.
Even if IMU timing is God_Time - it is only loosely coupled to GPS measurements. PPS interrupt cements that. IMU and Teensy clock timing will drift over multi-seconds - but not sure how that affects inter-second calcs?
IIRC my two T_3.6's had a cycle count diff of ~2000 in a PPS second but I didn't catalog that. And over time a given T_3.6 was drifting at least 500/240MHz against PPS. Seems IMU would drift as well unless temp controlled clock.
I've noted a thing or two that suggest using i2c_t3.h might be worth checking against wire.h for MPU9250 - is there overriding value in keeping it wedded to wire.h for 'generic' Arduino support?

Mike- quick scan of posts and CodeCompare::
You posted MPU9250_INS9State_V1.zip - prior name was MPU9250_INS9State__V5? I just downloaded and will 'compare' but too early not to be confused by name change.
Compare now shows you did merge and backdate the name? but you missed:

void readGPS(){
    if([B]GPS_PORT.available() &&[/B] gps.read(&uBloxData)) {

Will look at use of : gpsDataRdy = true;
Note sure why a redundant? - GPS data printed on any success w/debug: debugGPS1();
void getGPS() : looks to be doing extra work even if readGPS(); did nothing? - the float recalc of the utcTime?

Overnight of 7 hours my 168 MHz run shows 258 missed GPS events - 7 hours would be 126,000 GPS reports and that looks like 0.2% failure:

----- 258 ------------------------------------------- @17:27.59_-302878 >> 1== ms_0

Some failures may be DUAL - but I expect one fail can beget another. Not sure yet what triggers them with all the reads I added - except some execution anomaly.

 

[defragster]

Mike, Tim,
What are your thoughts on Brian's post above on his suggested approach?
1. Do a time update (i.e., kfProp) at IMU rate and buffer
2. When new GPS meas arrives do meas update (i.e., kfUpdate) for appropriate IMU reading
3. Re-apply time update (i.e., kfProp) from GPS meas time to current time

I'd need to make sure kfReset is run at the right time with the right flags, but seems pretty straight-forward.

My idea was a bit different (create a RTC via GPS PPS) but Brian's looks easier, I think...

I'll trust you guys to resolve the value of time as I don't have the cycles for the guts of the filter requirements/evolution, I'm just looking to get the most solid time base to decide on. If the time base is squishy - any use will be compromised. The IMU and GPS interrupt give true arrival of latest data ( plus variable CPU interrupt jitter and offset of device Sample to Delivery of calculated measurement ).

I do find the GPS jitter measure to measure disheartening - given I saw when my unit has a decent # of sats and is firmly seated the jitter suggests integrating it when static is suspect - and when moving ????

Reading Brian's note - my understanding says once GPS data is inbound the IMU data needs to be buffered for that 3ms period. Then using timestamps all measurements need to be fed through. That PAUSE on IMU manipulations would also assure GPS data is always received so the value it offers is fully realized. That would require a buffer system if IMU rates are perhaps 200 Hz or higher to prevent losing an IMU update. I might be able to try this in the current code if current IMU rate won't cause grief?

Note the cumulative impact time of : "kfP__ uS=2769 kf_U_ uS=2496" [kfProp() & kfUpdate() with kfReset()] is over 6ms. I'm not sure how often that is done - they are inline each loop() - but if done on 'old IMU data' with 'GPS inbound' - will impact GPS reception and utility.

 

[defragster]

Mike: the IMU interrupt is not recording the 'now' time again? IF IMU data processing is delayed during GPS reception getting 'now' LATER - won't be correct.

void runFilter() {
  newIMUData = 1;
  IMU_RX_time = tsIMU;
}

isn't it better with:

void runFilter() {
[B]  now = micros(); // This is when the data reported READY[/B]
  newIMUData = 1;
  IMU_RX_time = tsIMU;
}

<edit>: My first run of the latest _V1 code is not showing any Dtime output? Only the debugGPS timing????
>> My INT PIN was showing 25 not 50 in A_confingdefines.h
<edit2>: printRate was also back to too fast at 25 not 50 - even at 50 the GPS fail is higher?

 

[mjs513]

Tim:
"You posted MPU9250_INS9State_V1.zip - prior name was MPU9250_INS9State__V5? " - the MPU9250_INS9State_V1.zip should be the combined changes from the MPU9250_INS9State_V5 and the changes I made to the 6State for clean up, consolidation and putting additional GPS reads if necessary - idea was to break up the code in logical places and check if the GPS was updated. Not sure how successful but the 6 state seems to running well. The number of additional GPS reads (and getting rid of the extra IMU) is down to 4 additional all around associated with the KF implementation.

"Compare now shows you did merge and backdate the name? but you missed:" - I will get that updated and post in the next revision.

"the float recalc of the utcTime?" - got me - forgot about that one to be honest - wanted to make change but to be honest just forgot.

"Some failures may be DUAL - but I expect one fail can beget another." - I did notice that the fails seem to come in spurts. I couldn't figure it out either that one reason to add the extra gps reads all over the place and then reduce them.

"Some failures may be DUAL - but I expect one fail can beget another." - I am in the same boat as you on this one.

"That would require a buffer system if IMU rates are perhaps 200 Hz or higher to prevent losing an IMU update. " - that is interesting, I am not concerned with the speed except where the timing of getting gps reads may be affected, but it should work. I just don't know how to implement.

 

[mjs513]

" IMU_RX_time = tsIMU;" tsIMU comes from the elapsedMicros timer that never gets reset which I am using just for printouts on when IMU time was received. Thought this would be the same as setting tsIMU = micros() without having to worry about rollover.

' My first run of the latest _V1 code is not showing any Dtime output? Only the debugGPS timing????" - OK. That one is strange will have to check, should be printing DTIMES?
"My INT PIN was showing 25 not 50 in A_confingdefines.h
<edit2>: printRate was also back to too fast at 25 not 50 - even at 50 the GPS fail is higher? " This may have been me changing the wrong config I will fix if different. I tested at 50 and saw the same issue.

Give a little while to check and fix - have something to take of first.

 

[mjs513]

Tim. Updated to v2:
1. deleted the extra utcTime calculation
2. added back in the GPS_PORT.available test
3. Fixed your INT_PIN and printRate (not really sure how they changed).
4. My Dtimes are printing to Serial1 without out a problem - check to see if your coutD port changed as well.
5. Have to redo reducing the number of GPS reads again - was giving me a problem so I put them back in