uNav AHRS

Status
Not open for further replies.
Ok Here is version that kinda of works same issue as before. I saw your post and looks like Chris O. has a solution. Did find a new serial command not sure it applies Serial.send_now(). Think you put it after the cout command? Never saw that one before tonight.
 

Attachments

  • MPU9250_FiltersGPS_V10b.zip
    32.2 KB · Views: 143
Indeed .send_now forces the output - and waits until it is all buffered for send. Not sure if that would help.

DOH! - Need to go thank Chris! I needed to read my own code! I had Serial1 on the brain - the DEBUG port on mine ... where GPS is on Serial2!

Got the V10b - will see if I can't make it go before lights out.
 
With Chris' tip the Error average is 8 uS off from the RX_isr() detection now and worst case error is off by all but a full character of 21 uS where the last bits not complete. I could add the 8 uS into the calculation and call that good - remove the Rx_isr() code and take some load off the system? Simpler code and should always work [-9 to +11] uS to detect GPS start message if that is acceptable accuracy?
 
Updated code and notes see: Serial1-available-counting-bytes-in-the-FIFO

It looks like we can stick with the Rx isr()! It is running 100% good after 12,500 messages at 96 MHz on T_3.6! So we have a low overhead ( 5 Hz interrupt instead of hundreds or thousands of hits in <15 ms ) - that marks the GPS string start and time stamps it with less code than before.

Mike - after that I will not get to the newest code you dropped tonight - but you can swap this 'smaller' isr() code and it should work for you on T_3.5 at 120 MHz!
Code:
#define RxGapTime ((F_CPU / GPS_BAUD) * 30)
//==================================
// FASTRUN void GPS_serialrx_isr()
// GLOBALS :: volatile uint32_t GPS_RX_Time = 0;
FASTRUN void GPS_serialrx_isr() {
    GPS_RX_time = tsGPS; // Start bit transition detected after 'idle' time
    detachInterrupt(digitalPinToInterrupt(GPS_SRX));
    GPS_newData=1;
    GPSrxs++;
}

And then when a GPS read completes positive the interrupt must be attached again!:
Code:
if(gps.read(&uBloxData)) {
  // ...

      attachInterrupt(digitalPinToInterrupt(GPS_SRX), GPS_serialrx_isr, RISING);

  // ...
}
 
Last edited:
Tim. Hope you got a good night sleep. I updated revb to revc that incorporates your changes. Like you said works like a charm :) Now have a better way to determine when GPS is available. Will have to get that change incorporated as well.

There is also an improvement to the number of miss gps prints, better not perfect. Just as a side note I made the same changes in the threaded version, just for my own eddification. All these issues we are having is all related to how fast we can print - not sure when we get to final version we really need to dump all the data we are which will be even better.

EDIT: Think you mentioned that you are able to see the params in ucenter and make changes to them with your passthrough. I can't seem to do that with my pass through code. If you get a chance can you take a look. I tried a similar approach and that didn't work either usin inchar. Probably did it wrong.
 

Attachments

  • MPU9250_FiltersGPS_V10c.zip
    32.1 KB · Views: 93
Last edited:
Mike: Not quite a good sleep yet. Using the Threaded at 96 MHz it ran no trouble?: "120355.00==120355 | 0.246000, 61720.800700 ..."

Filters_V10c is compiled and running now T_3.6 @ 120 MHz.

Not sure what it showing on output - the GPS Rx isr() seems to be always hitting - but GPS data completion seems to lag or skip one? This is the opposite of what I saw before with GPSrxs now Higher? But that will happen when imu update arrives and the serialDataOut() is called after the GPS msg starts triggering the Rx isr() - but before GPS completes.

> Made minor changes for debug serial in A_Config.
> Not seeing any aberrations in print now not Threaded
> IMU update rate looks odd with 6 or 7 or 20 IMU updates between GPS? At 240 MHz I don't see the runs of 20 -
> Not sure what this test is looking for: if(timeStamp*0.001 > 235.0 && timeStamp*0.001< 275)

Here is minor update for now: View attachment MPU9250_FiltersGPS_V10c (2).zip
 
Hi Tim. As for sleep - me either, am shot. Anyway back to point.

> "Using the Threaded at 96 MHz it ran no trouble?: "120355.00==120355 | 0.246000, 61720.800700 ..." - made a few other changes to match what I did in non-threaded. Didn't post that version. Mainly limiting the prints. The no trouble was associated with the ISR. Still had about 5 missed prints for GPS update in about 4500.

> "Not sure what this test is looking for: if(timeStamp*0.001 > 235.0 && timeStamp*0.001< 275)" -I modified the debug print, to include a test for when serialPrint is not keeping up with the GPS. What is used was the whether the timestamp exceed the normal max timestamp, so hence, the if(timeStamp*0.001 > 235.0 && timeStamp*0.001< 275 test, max timestamp I've seen for each print (just print) was about 200ms between gps updates. the max value was just so the counter wouldn't keep counting on a miss. So in effect what you see in the last column of the debug is the number of times the printout wasn't keeping up with the GPS update. I put it there mainly because I got tired of watching the screen.

The other thing is that if you look at what I did to your debug print statement - I took the print for the pairs missed out of the "if" statement (was just easier for me to see when it missed).

> "Not seeing any aberrations in print now not Threaded" - I cheated I put am only printing IMU data every 25ms (put the imu print's in "if(sincePrint > 25)" block. Why 25, because I watching the data print out so much.

> "IMU update rate looks odd with 6 or 7 or 20 IMU updates between GPS? At 240 MHz I don't see the runs of 20 ", probably the same reason, I put a limit on the printed data.

The other thing I did, not sure if it made a difference was put a cout.send_now() after each print. I will give your updates when I get home - have to go out and do some errands :(
 
Just compiled and ran your updated version. Interesting changes you made to serial debug. Shows exactly whats going on. Noticed with the sincePrint there are more missPairs that show up that would be equivalent to missed prints which I didn't catch before.

It appears that when GPSrxs lags behind (seems to be always by 1) the ISR is when we get missed prints:
Code:
1807.00==1808	 |tS=208.68	 - 1.00 - 0 - 
1807.00==1808	 |tS=248.66	 - 1.00 - 0 - 
1807.00==1808	 |tS=288.65	 - 1.00 - 0 - 
1807.00==1808	 |tS=328.63	 - 1.00 - 0 - 
1807.00==1808	 |tS=368.62	 - 1.00 - 0 - 
1807.00==1808	 |tS=408.60	 - 1.00 - 0 - 
1807.00==1808	 |tS=448.59	 - 1.00 - 0 - 
1807.00==1808	 |tS=488.57	 - 1.00 - 0 - 
1807.00==1808	 |tS=528.56	 - 1.00 - 0 - 
1807.00==1808	 |tS=568.54	 - 1.00 - 0 -

This would probably equate to about two actual misses (say 0.2 and 0.4 gps updates). Not sure what else was changed, don't think anything else, but seems like we are getting a lot more misses than I saw before. Want to check something next.

UPDATE: Just looking at the debug you know pairMiss never gets updated it always 1.0. Also, you if(pairMiss), isn't that a Boolean test? Never saw that before so incMissed never gets printed. Not sure what you really wanted to do with the changes so just letting you know for now.
 
Last edited:
This shows UTC time when updated.
View attachment MPU9250_FiltersGPS_V10c (4).zip

It is possible the Filter match and prints are causing loss of GPS data - corrupted string will be tossed until next good one comes in. Since the isr() interrupt is off - it won't restart until a good GPS string completes.

<EDIT>: GPS strings not LOST/corrupted will just be found out of time and show in the next second.

This is why I was saying stopping filter updates (prints?) when GPS_newData=1; is set would help.

As that would also sync the IMU data with that GPS data.

ALSO - the send_now() assures integrity of THAT string - but can halt the system until it happens - which will slow the flow past the GPS uBlox reading.

The check for 1000 should be >=:
Code:
      if ( GPSMon >= 1000 ) {
      GPSHz = 0;
 
"This is why I was saying stopping filter updates (prints?) when GPS_newData=1; is set would help." - Agree that would probably help, i.e., prints.

"the send_now() assures integrity of THAT string - but can halt the system until it happens - which will slow the flow past the GPS uBlox reading." - thanks for the explanation, didn't get that from what I was reading.

"if ( GPSMon >= 1000 )" - ok you lost me on this one.

OK. Missed 2 updates while I was updating my last one. This is almost real-time. Forget what I said, I will try your latest before I say anymore :).

Have to take a break for now and go make my granddaughter something to eat. What can I say :)
 
I'm breaking too. In the void serialDataOut() code search 1000. That '>' should be '>='. I didn't see until after the zip was packed . . .

Look for function .availableForWrite() that shows buffer space free to push output. When it is less than written - there will be a wait with send_now().
 
Since the Rx is its no longer free running, the count doesn't get ahead at the start, took out the - 1 in it value, and it never misses anymore.

When right is up 1 then GPS was not read or a message was lost
 
You are right about it never missing anymore. Was just interesting that when it did in the last version you posted was when the data was off. I was looking a the latest version in post #460, compiled and ran fine but had a devil of a time seeing in the debug monitor when the prints went off. I did remove the send_now's based on what you said and I put a test on serialDataOut thus:

Code:
      if(!rtk.upDated)
          serialDataOut();


Looking at the data monitor saw lots of loss of sync with the GPS. In the debug monitor the only thing I saw was
Code:
2372.00==2372	 |tS=0.04 | 	 @81324.6015625000	 ms196==1 || 
2373.00==2373	 |tS=0.04 | 	 @81324.7968750000	 ms392==2 || 
2374.00==2374	 |tS=0.04 | 	 @81325.3984375000	 ms991==3 || 
2375.00==2375	 |tS=0.04 | 	 @81325.6015625000	 ms1190==4 || --------

As you said the ISR's never missed a beat, but notice the utc times. Lost data before utc 81325.3984375000. Only indication I saw in the monitor was that count at the end only had 4. this number or less indicated a data miss. again a lot of times that I could see.

EDIT: are we spending too much time trying to get the timing down on prints just so we can print to tViewer? Do we need to print all that data? Isn't the real question whether the GPS data is avail at the right time,i.e., GPS data isn't being lost?

EDIT1: Think another reason I may be seeing more misses is increased logic and prints to Serial1. Its going to take more time in the print loop.
 
Last edited:
Hi Tim. Sorry about my tirade but getting frustrated with getting to work. I did some more work and ran a couple of test cases. The first was without the !rtk.updated on the imu serialPortOut (figured this would be the fastest print) and the second was with it. I did modify your debug statements to reduce the amount of prints to coutD port. I also moved the missed GPS indicator to outside the test if the gps was updated (all misses occur between updates, I wanted to get a running count. Anyway, in the first case an example is of two misses between tracked updates:
Code:
3 **** 102.00==102	 |tS=0.04 | 	 @4847.4003906250
4 **** 103.00==103	 |tS=0.04 | 	 @4848.0004882812
5 **** 104.00==104	 |tS=0.04 | 	 @4848.6000976562
5 **** 105.00==105	 |tS=0.04 | 	 @4848.800292968
I know its between because I ran a test where I printed the misses as they occurred. You can see, when the rtk.upDated = true, the misses in updates already occurred yet the isr count equals the gps.read count. You can see the impact on utc. 2 misses in the first and 1 in the second.

The second case, I got:
Code:
91 **** 3244.00==3244	 |tS=0.05 | 	 @5716.0000000000
92 **** 3245.00==3245	 |tS=0.05 | 	 @5716.6000976562
Same result but the thing I noticed was miss count was less. Right now with about 6200 hits getting about a 2.6% miss rate.

One other test I ran was using the serialDataOut function modified to only print 3 fields, the first 3 timing fields. All worked as advertised. After about 2000 updates there was 0 misses. What to try something - if imu is updated only print IMU data, if GPS updated only print GPS data. All other fields would be zero. Only problem with this is again it would impact viewing in TViewer.

So it really boils down to what we really need to viewed and when. Can we speed things up and print all integers and adjust with the scale factor in TViewer?
 
Yeah - frustrating - and may flow a bit better on a T_3.6 - but we should not have the system bound on USB prints to tViewer.

Using the coutD - debug - separate channel and buffer - especially for lower volume stuff should have little impact as long as the buffer doesn't back up. Could try upping that to 3 Mbps - but USB is closer to or over 10 Mbps net output. IIRC - can set to 4Mbps but a test Paul showed it being less efficient at some point and not actually going faster at 4 or over.

That cleanup on the isr() made it hit 5 times per second - and be disabled in between - where it was pinging perhaps thousands of times per second - and more involved code checking time base and doing a conditional. But until a complete GPS finished - the isr() will not report on any other incoming messages to indicate they are missed as it is. So the true test is completed GPS messages. Having the system watch for the Start detect and then delay other tasks after 1 ms until the message is completed in 2.2 ms will assure they are caught and allow ~197 ms to do whatever before the next GPS message. TThreads could help with that - but that was showing those anomalies on my system where output was interrupted - and taking too long finish one cycle before the next started.
 
Just had a thought - I see some 18 values {40 bytes counting 2 doubles} printed here that expand into at least 133 bytes with commas, space and newline:
Code:
  cout.printf("%10.6f, %10.6f, %10.6f, %8.3f, %d"    ,timeStamp*0.001, rtk.utcTime, IMU_RX_time*0.000001, _dt, rtk.fixType);
  cout.printf("%12.8lf,%12.8lf,%10.4f"                      , rtk.lat1, rtk.lon1, rtk.hMSL);
  cout.printf("%10.4f,%10.4f,%10.4f"                        , rtk.velN, rtk.velE, rtk.velD);
  cout.printf("%10.4f,%10.4f,%10.4f"                        , ypr[1], ypr[2], ypr[0]);
  cout.printf("%10.4f,%10.4f,%10.4f\n"                     ,aNEDx1, aNEDy1, aNEDz1);

Having looked at the uBlox parsing code for the PVT 'compact' message of 100 bytes. If those 40 bytes - call it 50 with some overhead - were shipped off to a 2 Mbps serial port to the DEBUG Teensy to unpack not only would that reduce the amount transmitted ( yes over a slower bus ) - but all the floating printf conversion processing overhead would be on the dedicated Teensy as well - that would extract and port the data to it's USB to send to TViewer.

Serial at 2,000,000 bits per second could transport 4,000 set of 500 bits in a second ( where each byte transports as 10 bits with Start and Stop ). Ideally this would allow 500 to 1,000 transfers from the test Teensy to the Debug Teensy - now the TViewer Teensy with less time wasted processing and transmitting the output in TViewer readable form.

I could do a quick test to see how much time is spent in the #1: Formatting and #2: Transmission of the data currently processed in the serialDataOut() function. Hopefully the 50 byte binary update would be much less intense.

I wanted to write this down before I forgot of it.

Ideally - this could also be done over SPI using DMA at 2-10 times the speed where a block of memory on Teensy #1 is perpetually updated on Teensy#2. Assuming they could be Master and Slave and somebody with the knowledge to do that made it happen ( not me unless I found a sample ). If 500 bytes were reserved on each side - 10 sets of the values could be updated in a loop on T#1 and they would just refresh on T#2 under DMA control.

tonton81 has a Master to Slave SPI thread I posted on - not DMA - but the right idea perhaps. Got inital reply already.

KurtE was doing something with this IIRC ...
 
Wow you been busy. Don't know how you do it. Here is another test point. I made some changes to the threaded version just as a test and fell asleep so it ran over night. While I still got some misses the error rate was 0.6% vs the close to 3% with the unthreaded: [640 **** 106416.00==106416]. This was with the string data print. Going to test the double version now
 
post #467 was just an idea - that only took a moment . . . it seems easy enough to do - and it should be . . . my reality may be different :) Either way it won't be just now.

Indeed with Care Threading should provide some benefit for time sharing - but easy to get wrecked. Though - so it seems is 'normal coding' of this output overhead.
 
Sorry if I implied you were going to do the things in post #467 now. I was still half asleep when I wrote #468. I agree with the threading, you have to be careful. Would rather stick with the non-threaded version first. Going back to the threaded version was just as a test more out of curiosity (if you want the updated copy I can post). Now that I am awake going to go back to the non-threaded want to do some experimenting. The key really is what you wrote - "we should not have the system bound on USB prints to tViewer.". Hope Don can provide some input on this since the data out was what I am presuming he needs to see.
 
If you want to give the updated threaded version a try for curiosity purposes I am attaching it. PS. No matter what I tried can't get the gps misses down. To use the ISR we just have to test on gps_newData correct - have an idea but the logic gives me headaches.
 

Attachments

  • MPU9250_FiltersGPS_V4_threaded.zip
    33.3 KB · Views: 70
Thanks, saved.

Indeed - when a message starts and the GPS_serialrx_isr() is active then GPS_newData=1 will be set.

QUICK NOTE: I do not find where : "GPS_newData=0;" in my copy of "MPU9250_FiltersGPS_V10c\*.*" and that should be done when: "if(gps.read(&uBloxData))"

At "GPS_BAUD 460800" the GPS message will be complete within 2.25 ms after GPS_serialrx_isr() sets GPS_newData=1

Perhaps in loop() if (1==GPS_newData) { readGPS(); cout.send_now(); } readGPS();

This would empty GPS msg incoming , PAUSE the system to clear the output buffers, then empty the GPS msg incoming, and return to loop() processing and quickly get the completed GPS message, and have empty output buffers for what follows.

Then serialDataOut() should perhaps on entry " if ( 1 == GPS_newData ) return;" - we can refine this later to queue the output, or with alternate output method it will get fast enough to not mess up the timing. It would not halt filter data reads and updates for now.

This would sacrifice a TViewer output update (or two) and assure the fresh GPS is fully retrieved ASAP, and Teensy ready for output and normal loop()'ing for the next 196 millis.

Today is my wife's birthday - with the gift of snow! :) - so I should refrain from being here today. Hopefully this will be enough for an attempt on your end if time allows?

I'm not sure what you use as an editor - but I've moved to SublimeText external to IDE. It allows global folder search I've found critically useful going back ~25 years when I had a job and used another editor that gave me that across projects bigger than Arduino+Teensy sources - Praise be to SourceInsight! I always have an instance of SublimeText open on "T:\arduino_1.8.5\hardware\teensy\avr" and can quickly find anything TeensyDuino installs in the PJRC CORE, Library and Examples sources. And another instance open on current project - like the current download of : T:\tCode\_GPSimuDK\MPU9250_FiltersGPS_V10c.
 
First wish your wife a very happy birthday for me. Tell me about the snow -- turned out to be mostly a layer of ice on top of water. Still sore :)

I noticed that GPS_newData wasn't reset but just wanted to be sure that was the intent before I mentioned it.

Was thinking about how I could do a test for new data and you just gave me the answer a lot more eloquently than I would have implemented."if (1==GPS_newData) { readGPS(); cout.send_now(); } readGPS();" would have to go at the beginning of the loop I assume. I have been going back and forth between the ide and notepad++. Never told the ide to use it as the editor though. I will have to check out sublimetext and how to do all that fun stull. I actually have a code compare application that can check for differences in versions that I started using.

Anyway go enjoy the day with your wife. Its her day.
 
Thanks - will do. We get little snow at the house - 2nd time this season - none last to speak of. I grew up in PA - left to the less extreme west and stayed here.

Indeed GPS_newData needs to toggle to have usable value - DOH! - took a long time for me to notice that.

Yes - top of loop() where readGPS() is now could be replaced with that "if ()... line" - the second one doesn't "need" to be 'outside the if ()' EXCEPT for sanity in case for some reason GPS_newData gets out of sync.

As long as the loop() flows smoothly at least 1K Hz having readGPS() at the top like that will complete the GPS msg within 3 ms. I often put a loop()/sec counter in on an elapsedMillis so I can have a feel for that.

BTW: is there a "void yield() {}" in the main INO?

I didn't do much code the first 15 years of my vacation - used Notepad++ too - even though I have 2 licenses for the old SourceInsight I had forgotten much of what I knew of it and it was getting old. KurtE pointed me to SublimeText and I bought a 'user license' because it had folder grep. It has add on's like SublimeMerge that allow DIFF checking I've used a bit - but not paid for and in free trial again on this new computer. Not as handy as the old cmdline diff/merge tool my day job had - still looking for that.
 
I will give it a try later - kind of have a three person birthday celebration today myself (3 people in family have birthdays on the 14th, 21st and the 28th).

No yield in the main ino - will add that in.

"As long as the loop() flows smoothly at least 1K Hz" - learned something new again here - have to remember to check that.

Yep - the cmdline diff/merge and grep were handy (used unix a bit). Besides CodeCompare, which does diff it will allow you to do merge as well) I use grepwin a lot. Handy tools.
 
Status
Not open for further replies.
Back
Top