uNav AHRS

[mjs513]

I typically run it with an SRD = 9, but I have ran it at 4 also. Haven't tried anything else.

 

[defragster]

Don: T_3.6 getting 100 Hz IMU data right now. if kfProp() keys on getGPS() that could be tied to that. make void readGPS() and getGPS() into bool's() and propagate that { or use the rtk.upDated or gpsDataRdy flags } and do it right there?

Added this Dtime monitor code:

Dtime( 6, 0, 0 );
      serialPortOutKF();  // String print function for Telemetry Viewer
Dtime( 6, 10, "sPout" );

And it is showing micros of "sPout uS=1518" to "sPout uS=1701" typical. That is a lot of downtime - and then the data pushing out USB.

The SPI Slave Teensy for output may soon be online thanks to tonton81. And it may eject a byte array from Master to Slave in under 500 uS [maybe 230 uS] - then the Slave would format and feed TViewer the data for updates without the 1.5 ms wasted time filling up the USB port - which could then be used for faster native debug spew - and at least twice the data updates Hz and perhaps more data values could be sent to TViewer with even less impact on the Master IMU operations.

 

[Don Kelly]

I moved the kfProp to the IMU section, left the kfUpdate in the GPS section, and moved kfReset to the tail end of the IMU section, and the KF appears to be cycling somewhat reasonably now (residuals ~1 to 2 m).

I'm seeing five printouts at each GPS time, and the residuals are different for each of the 5 printouts with the same time, yet kfUpdate should only be called once per GPS read so the innovations shouldn't show five different values at the same five timestamps. I need to go think about this...

I also need to go and do that serialX mod y'all suggested..

Running out to dinner, so that may be it for tonight for me...

Just looked at the residuals now, and they're on the order of ~0.2m, so promising!

Don

 

[defragster]

I was confused by '5 printouts with the same time' - then it clicked - the second is unchanged for the 5 Hz updates.

I did append the _nano portion to my 'simple/minimal formatted' digital ( not UTC converted ) H:M.S_nano in : #25835 (! 12) @23:24.25_199923469 >> 2== ms_199
{ and each is indexed in #25835 and if this number (! 12) ever increases after 'startup' seconds of buffer catchup then messages are being lost from the expected 5 Hz rate }

Yes, increasing the GPS Rx Serial buffer is critical - and as 'easy' as swapping 64 to 128 ... at the right spot ... for each TeensyDuino install/update.

 

[Don Kelly]

Tim,
It's printing like this:
85059.00
85059.00
85059.00
85059.00
85059.00
85059.20
85059.20
85059.20
85059.20
85059.20
etc

So it's showing the 5Hz changes, but just 5 printouts at each 5Hz step, where there seems like there should only be one. It wouldn't bother me so much if they were 5 copies, but the residuals change at each of the printouts, and innovations should only be calculated once with each GPS meas...

 

[defragster]

... update your "GPS_PORT Serial3" ASAP.

>> ...\hardware\teensy\avr\cores\teensy3\serial3.c

Should be line #43 :: #define SERIAL3_RX_BUFFER_SIZE 128 // 64 // number of incoming bytes to buffer

<edit #2>: I also see 'GPS_BAUD 115200' so the GPS data is feeding in 4X slower than "GPS_BAUD 460800" - so it will take almost 9ms for a string instead of 2.2ms. The good thing is that may be making your 64 byte Rx buffer last longer before overrun.

And ideally if you could change to Serial1 or #2 with FIFO the Teensy will have fewer UART interrupts to catch GPS bytes.

<edit>: Looks like I should edit Dtime() to have an easy way to disable that usefully annoying display of how long stuff takes as it makes watching GPS updates harder.

<edit #3> : a quick hack to make Dtime() quiet would be to change #ifdef coutD to #ifdef XX_coutD in MPU9250_INS9State_V3.ino

uint32_t Dtime( int ID, int State, const char *szID  ) {
  uint32_t RetVal=micros();
#ifdef XX_coutD

----- #241 (! 10) @23:53.33_-437853 >> 1== ms_65553
#242 (! 10) @23:53.33_199562218 >> 2== ms_65753
#243 (! 10) @23:53.33_399562291 >> 3== ms_65949
#244 (! 10) @23:53.33_599562364 >> 4== ms_66151
#245 (! 10) @23:53.33_799562436 >> 5== ms_66351

----- #246 (! 10) @23:53.34_-437490 >> 1== ms_66552
#247 (! 10) @23:53.34_199562581 >> 2== ms_66752
#248 (! 10) @23:53.34_399562654 >> 3== ms_66953
#249 (! 10) @23:53.34_599562726 >> 4== ms_67154
#250 (! 10) @23:53.34_799562799 >> 5== ms_67352

 

[mjs513]

The SPI Slave Teensy for output may soon be online thanks to tonton81.

I was been going through that thread today, and saw the back and forth between you and tonton81. If you could get DMA working that would be really make it interesting. In the meantime I found this on Amazon that looked interesting also, https://www.amazon.com/gp/product/B00HKK4SCO/ref=ox_sc_act_title_1?smid=A20H7HZMYKP2JK&psc=1.

I'm seeing five printouts at each GPS time, and the residuals are different for each of the 5 printouts with the same time, yet kfUpdate should only be called once per GPS read

Sorry for the confusion. The first column is the utcTime of the GPS read and stays the same for each GPS cycle. The second column of the printout is the tsIMU timestamp for when the IMU interrupt hits, and the third is the tsGPS is the timestamp for when the interrupt hits for the GPS updated. utcTime and tsIMU should not changes unless you have new data.

EDIT: I did what Tim suggested and made a big difference. Take a look at Don's and mine A_configDefines settings.

 

[mjs513]

Don,
Not really sure where you wound up putting "I moved the kfProp to the IMU section, left the kfUpdate in the GPS section, and moved kfReset to the tail end of the IMU section, and the KF appears to be cycling somewhat reasonably now (residuals ~1 to 2 m). "

Mike
,
EDIT> Tim. Made your edit and running a little lower than you, about 917, oops mean 971 but I am on T3.5

 

[defragster]

Perhaps I was looking at the wrong output window - but the above debug confirms the time when each GPS is received without error.

Yes, tonton81 just posted the first release version of SPI_MSTransfer. I'll get setup to test that 'soon' and then see if I can understand it and then try the DMA update.

Even without DMA: 1ms per update is gained moving TViewer parse/print to second Teensy and will unburden the Master - and make code uploading easier ( not sharing USB Program<>TViewer port ). TViewer will run off the Slave - if everyone has one ( T_3.5 or T_3.6) - and then coutD can go back on Serial for ~6X faster throughput.

 

[mjs513]

Man, tonton81 has been busy. Missed the posting while I been doing this. Yes you are right of course.

 

[defragster]

Mike: "EDIT> Tim. Made your edit and running a little lower than you, about 917 "

opps .... I was running at 48 MHZ!

The 240 MHz time is more typically about: "sPout uS=330" up to 384 uS.
Much lower than I expected - thought it measured higher before ( testing at too many clock speeds ). So it may help less than expected at 240 Mhz - but 230 uS without DMA is still better than 917 for you Mike, and the Master Teensy won't be overwhelmed printing out USB during development for TViewer code not used later.

I'll focus on DMA version ASAP that should take (well) under 100 Us if it can work.

 

[defragster]

Man, tonton81 has been busy. Missed the posting while I been doing this. Yes you are right of course.

tonton and me both busy - since I posted here and there with him ... got chores to do . . .

 

[tonton81]

defragster, sorry to hijack, but i got 106uS at 24MHz for a 100 byte payload just by F&F and dont even wait for the ACK.. this is also an option, think of it like UDP (without DMA)

 

[defragster]

defragster, sorry to hijack, but i got 106uS at 24MHz for a 100 byte payload just by F&F and dont even wait for the ACK.. this is also an option, think of it like UDP (without DMA)

No apology needed - to me ... I hijacked this thread long ago ...

Additionally - that is awesome news - and this is where it will see first use outside your lab . . . I'll just need to figure out how to have the Master submit and the Slave receive the &TViewer_Struct and get it wired up.

48 bytes plus CRC etc ... ~75 uS ???

The current TViewer output will need these 12 RAW DOUBLE values packed into 48 byte TViewer_Struct and over to Slave and then converted, formatted and Serial.print()'ed:

  dtostrf(rtk.utcTime, 10, 6, utcText);
  dtostrf(IMU_RX_time*0.000001f, 10, 4, tsIMUText);
  dtostrf(GPS_RX_time*0.000001f, 10, 4, tsGPSText);
  dtostrf(RefLLA(0,0)*rad2deg, 10, 6, latText);
  dtostrf(RefLLA(1,0)*rad2deg, 10, 6, lonText);

  dtostrf(RefLLA(2,0), 10, 4, altText);
  dtostrf(sqrt(pk1(0,0)), 10, 4, pk1xText);
  dtostrf(sqrt(pk1(1,1)), 10, 4, pk1yText);
  dtostrf(sqrt(pk1(2,2)), 10, 4, pk1zText);
  dtostrf(nuk1(0,0), 10, 4, nuk1xText);

  dtostrf(nuk1(1,0), 10, 4, nuk1yText);
  dtostrf(nuk1(2,0), 10, 4, nuk1zText);

  // Create single text parameter and print it
  snprintf(text, textLength, "%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s",
           utcText, tsIMUText, tsGPSText,
           latText, lonText, altText,
           pk1xText, pk1yText, pk1zText,
           nuk1xText, nuk1yText, nuk1zText);

<Edit>: tonton81 posted on his thread he had debug enabled - turning that off gave:

So I retested at 24MHz a payload of 60 bytes like before, without debugging:
1) with acknowlegment by crc verification -> 69uS
2) without acknowledgement (true F&F) -> 64uS

 

[mjs513]

tonton81 - don't worry about me, you can jump in any thread i'm on. All I can say is that this is going to get interesting. Now I have to go set up another T3.5. By the way would this work with a T3.2 as the slave?

 

[tonton81]

im not sure, as long as the slave registers match i dont see why not. afaik the t3.5 and 3.6 registers are same for slave, the master uses built in transfer16 function from core

 

[Don Kelly]

Hey all,
Attached is the version (I called it V5Tim ) with the kfProp, kfUpdate, and kfReset moved so that they're where I'm thinking they should be. To figure out where y'all had started/stopped the "if" blocks, I indented the code to be able to follow it easier. I also commented out (but left it in!) the DTime lines in this version. Please uncomment them back if you'd prefer, I was just keeping things as simple as possible while I moved the kf calls around.

In the morning I'll fix my serial stuff as y'all are suggesting. thx for the reminder.

Not sure what you all and tonton81 are talking about, but it sounds interesting!!

View attachment MPU9250_INS9State_V5Tim.zip

 

[defragster]

SPI is generally supported similarly on T_3.2 as the T_3.5/3.6 AFAIK - pins and all map the same. I was planning to wire it up on a T_3.5 to make sure it worked for Mike. A full speed T_3.2 should work as well.

I have t T_3.0 from the PJRC 'End of Life sale' as my Serial debug Proxy - it would probably work at 96MHz given the T_3.6 can do print and all else at 48 MHz. Though I was thinking a T_3.5 or 3.6 might be also able to log all real time data to SD card for later playback using the SPI_MSTransfer packets without disturbing the Master. Since SPI_MSTransfer is BiDirectional the code could be instrumented to use that to replace the real devices - or given the card run directly on any T_3.5/3.6.

I saved V5Tim. I'll add a disarm toggle switch to DTime().

 

[mjs513]

Don - Tim

Took at look at Don's updates. There's a difference in what I have been working with since I was doing code clean-up. Let me make the updates and repost the updated sketch. Example - all the readGPS's are still in there as well as a couple of missing tests. But now that I know where the changes are to the filter it will be easy to update. In the middle of home things so will update later tonight and repost.

Tim: Thanks for making sure it works for me. See I will have another project tomorrow

 

[defragster]

Mike: Will wait on newer V5Tim. I've got one other chore and then have to digest the SPI_MST code so no rush.

 

[mjs513]

Ok. Don - I couldn't even get the version you posted to run on my setup - it appeared the GPS was never read. I tried to incorporate your rearrangement of prop=update-reset, really didn't work for me. I am attaching the version we should all be working to now. The attachment says for Don since it really isn't updating. Didn't do any timing tests to see whats going on. Wanted to get it posted for those of you who are night owls - unfortunately I am not.

 

[defragster]

My other chore done . . . now to look at the gift I asked for . . . SPI_MSTransfer ... T_3.6 beside me ... have to go fetch the T_3.5 ...

Mike - V5Tim runs for me ? It has the wrong readgps.in:: debugGPS(). I swapped that and with Dtime off I see this showing GPS data arriving in good fashion:

-----		 #116 (! 10)  @5:25.17_-210017 >> 1==	 ms_40228
		 #117 (! 10)  @5:25.17_199790055 >> 2==	 ms_40427
		 #118 (! 10)  @5:25.17_399790129 >> 3==	 ms_40628
		 #119 (! 10)  @5:25.17_599790203 >> 4==	 ms_40828
		 #120 (! 10)  @5:25.17_799790277 >> 5==	 ms_41028

-----		 #121 (! 10)  @5:25.18_-209649 >> 1==	 ms_41228
		 #122 (! 10)  @5:25.18_199790424 >> 2==	 ms_41429
		 #123 (! 10)  @5:25.18_399790498 >> 3==	 ms_41629
		 #124 (! 10)  @5:25.18_599790572 >> 4==	 ms_41829
		 #125 (! 10)  @5:25.18_799790646 >> 5==	 ms_42028

Let me know who gets back to it first for the new baseline.

 

[mjs513]

Thank Tim. When you posted I remembered what I forgot to do - change the profile to my imu config - duh!!!!! Now all is well. Seems to be working but I do have a question for Don.

Don - was it intentional that you only wanted kfUpdate to run every 25 milliseconds/ just when we do a print out.?????

Mike

 

[defragster]

Thank Tim. When you posted I remembered what I forgot to do - change the profile to my imu config - duh!!!!! Now all is well. Seems to be working but I do have a question for Don.
...

I was going to mention ... there is this one "#define mjs" in : ...\MPU9250_INS9State_V3\A_ConfigDefines.h

but since you created it . . . that seemed a bit . . . .

 

[mjs513]

That's why duh!!! should have added a doh!!!!! as well. Too tired - but don't like sleeping until I get things resolved if you know what I mean.