uNav AHRS

[mjs513]

defragster. First test results: 14963.00==14965 with 3 misses early on. This is with timeslice 1ms for all threads. Going to run it now with your settings. I am doing something a little different than you are to ensure variables are thread safe. I am using atomics with load/store. I will post it for reference .

tonton81. Right now all serial prints are done from within the loop. If we start doing prints from the threads will definitely have to use a mutex. I remember your discussion on the multithread thread

Mike

UPDATE: I changed the timeslice to 10 ms and stopped real quick: here are the results:
! ! ! ! ! ! ! !! ! ! !! !! ! ! ! !!!! ! ! ! PairMiss18
1781.00==1781

Update 2: here is my updated code:

 

[Don Kelly]

quick update...
All routines built and they compile (woo-hoo!!)
- cTrans.h (coord transformation)
- kfINS6State.h (contains kfInit, kfProp, kfUpdate, and kfReset)

Now the debugging and testing starts... if anyone wants an interim release, let me know, but I'm assuming no one wants one until I at least get it cycling and looking respectable!!

 

[defragster]

Good news on your progress Don!

Mike - odd you get the PairMiss? I pulled your code and will give it a run soon. I see those minor string anomalies - but ZERO PairMiss - my 96 MHz T-3.6 now running at counts of: 29568.00==29568

The same anomalies on similar schedule in TyComm appear in IDE SerMon - so it isn't the SerMon program.

 

[mjs513]

Don, like Tim said. Great news you making such progress. Can't wait to see what you came up with.

Tim - I am not seeing any string anomalies but I am running from Serial4 to the PC and using a small python script to read and print the data. Curious about the pairs as well. Could be compile differences - what optimizer setting are you using?

 

[defragster]

I keep picking 'Fastest' for optimization - You?

96 Mhz T_3.6 now at "49222.00==49222 " and counting with zero MissPair groups of 10 shown - so 100%.

I'll bump Serial1 IN and OUT buffers on PROXY T_3.0 unit and see if that clears it up.

Also - in case the T_3.0 is the weak link - I can rewrite this to be biased toward reading 'PROXY_SER Serial1' in a WHILE and this code doesn't need to use the InChar. Oh, and I need to add the void yield(){}. If that fails I could use a RAM buffer to read the count .available() and then write from that as write .available allows.

void loop() {
  char inChar;
  if ( Serial.available() )
    PROXY_SER.print( inChar = Serial.read() );
  if ( PROXY_SER.available() )
    Serial.print( inChar = PROXY_SER.read() );
}

 

[mjs513]

"I keep picking 'Fastest' for optimization - You?" - I am on Faster. I will have to give Fastest a try.

" 'PROXY_SER Serial1'" - ok learned something new. Will have to give this a try on with the serial pass through I have to see if it reads the config with ucenter.

"49222.00==49222" - wow that's great news. I have been running at 120 the whole time. For the heck of it I should try lower speeds to see how it works. One step at a time I guess.

UPDATE: Don't use the file I just posted there are a few bugs I have to fix - made a couple of changes that didn't get carried through. I repost soon.

HERES IS THE CORRECTED VERSION:

 

[defragster]

Just started the execution with updated code below on the T_3.0 PROXY device - I also edited Serial1.c to have 128 byte Rx and Tx buffers.

You are using Serial4 - that does not have FIFO's ( like #1 and #2 ) - I wonder if interrupts from feeding the Serial4 might be affecting your code? Did you catch that I upped this #30 to 100? This number can be higher - and at 30 it might be double triggering.

FASTRUN void GPS_serialrx_isr() {
  //...
  if ( tt - xx > ((F_CPU / GPS_BAUD) [U]* 100[/U]) ) {

As I type the new PROXY T_3.0 is showing an anomaly at "425.00==425 " and 2027 and more, but no missed pairings over 2850 GPS prints. I have not moved to V4 Threads yet.

// For second Teensy connect USB to SERMON (TyCommander)
// Connect two Teensy GND's and cross Rx/Tx to either Serial 1 or 2 port here and in the other sketch.
#define PROXY_SER Serial1
#define PROXY_BAUD 2000000

void yield() {}

void setup() {
  Serial.begin(9600);
  while (!Serial && millis() < 4000 );
  Serial.println("\n" __FILE__ " " __DATE__ " " __TIME__);
  PROXY_SER.begin( PROXY_BAUD );
}

void loop() {
  if ( Serial.available() )
    PROXY_SER.print( (char) Serial.read() );
  while ( PROXY_SER.available() )
    Serial.print( (char) PROXY_SER.read() );
}

 

[mjs513]

"Did you catch that I upped this #30 to 100?" no I missed that one. I recopied your updated code so the change is in there anyway. I can tell you that I do see the skip in the data once in while but not like it was before.

"You are using Serial4 - that does not have FIFO's ( like #1 and #2 )" I forgot about that. Wonder why I didn't use #2? Just rhetorical.

UPDATED I changed over to Serial2 just in case, ran a short test but still got a few misses. If I change the 100 to 80 it almost dead on.

 

[defragster]

"Did you catch that I upped this #30 to 100?" no I missed that one. I recopied your updated code so the change is in there anyway. I can tell you that I do see the skip in the data once in while but not like it was before.

"You are using Serial4 - that does not have FIFO's ( like #1 and #2 )" I forgot about that. Wonder why I didn't use #2? Just rhetorical.

UPDATED I changed over to Serial2 just in case, ran a short test but still got a few misses. If I change the 100 to 80 it almost dead on.

Found the selective user in A_configDefines.h - Nice.

Compiled and ran - made the GPS debug output in serialPort optional to if(rtk.upDated == true) and left the IMU data run out USB - now I see the "PairMiss"!

Will give it a look - we might want to make a new thread for the threaded uNav-AHRS . . . to get this working

 

[defragster]

T_3.6 At 120 MHz With the whole TViewer output string to USB and/or Debug Serial I see the failure of the interrupt to catch all the time - the GPS string still comes through - but without the advance interrupt detection timestamp.

Bumping to 180 MHz and it runs fine. I even switched to the raw cycle counter - so it doesn't seem to be from micros() failing to keep time or taking too long.

There are 100 IMU interrupts per second, and that doesn't seem to be a problem. There will be more extraneous Serial Rising interrupts from 5 sets of 800 data bits - those are ignored based on time. And with the TThread switching I'm not sure how offline the CPU for that.

It would be functional/better perhaps to calculate the start bit by counting how many bytes are there on the first receive of a new message before calling the uBlox code with .available test before calling the uBlox::gps.read() - that would add less processor overhead given the message is only coming in across ~3 time slices 5 times/second. I'll give that a try.

 

[mjs513]

I made one change to your pairsmissed print. I commented out the pairs missed coutd statements and used this instead:

      coutD.print( rtk.heading );
      coutD.print( "==" );
      coutD.print( GPSrxs ); 
      coutD.print( " == "); 
      coutD.print(PairMiss);

At 144Mhz oc I am still seeing pairsmissed, but I am still testing some changes.

Just as a reminder I did change the code from your version to use atomic variables with load/store functions.

EDIT: Really watching the pairs missed data it appears that periodically the isr hit rate increases rapidly to catch up to the gps counter. The other thing is after a miss it resynced.

 

[mjs513]

Think I figured out why the prints don't always show the GPS updates. It boils down to serialportdata or for tviewer just simply takes too long, so periodically the loop thread misses the updated from the GPS thread. So, Don, what fields do you really need for your filter. I want to run a test with the simplified output to see if it still misses upates, of course the filter will probably take less time.

 

[defragster]

Indeed per my last post - even split USB and Serial - somehow the sum total of the output takes too much time. Showed the Rx isr() is on the edge though - at least with threading in the background. Will add the indicated code before GPS first read and compare them and if good dump the isr() on Rx.

Also suggests pausing filtering as the GPS data arrives (3 ms) - i.e. queueing the IMU data received and stored will assure the GPS data comes in when ready - then 197 ms of free time until the next GPS data string. So having notice of that would prevent overload. And even without threading it could be managed. Threading and keeping data safe across them quickly adds concerns and overhead.

It seems TViewer just plots each received point without regard to time right?

 

[defragster]

before I thought the Rx isr() would be better - which it is - I considered using this method that averages 13 uS error ( 1/2 char) at 240 MHz { though it can be off by 2-3 chars at times } { compared to the Rx isr() value) } {the error is the bits in transit or bytes not yet pulled from the FIFO by the Serial interrupt code } - at least it gives notice that GPS message is incoming and an approximate start time. Though that could be done without the math shown putting a value in GPS_State could just be 1 instead.

Here is the core snip that adds the detection to readGPS() in the threaded case - easily changed for the non-Tthread case - for testing purposes the value of GPS_State is the micros value when the message began { i.e. GPS_RX_time } Note: if the time slice ends wrong this math could get ugly wrong - so it would be best unthreaded - the while would be an if() and the .yield a return 0.:

    while ( 0 == (GPS_Avail = GPS_PORT.available() ) ) threads.yield();
    if ( 0 == GPS_State ) {
      GPS_State = tsGPS-((1000000*( 0+(GPS_Avail*10) ) ) / GPS_BAUD );
    }
    if(gps.read(&uBloxData)) {

When waiting for a fresh GPS string GPS_State is left '0' when a parse is completed, and thread yields when no .available - this is just polling like the normal code that would return. Time is recorded when characters arrive and then he uBlox string parsing runs.

If there was a GPS PPS signal we could tie UTC time from GPS to the Teensy readings for IMU - without that this is the closest I can see working?

Edit PS: Both Rx isr() and this could be done - seeing the isr() time off from this 'estimate' more than ~100 uS (or a flag) - means it did not catch the start bit.

 

[mjs513]

Hi defragster. I have to mull over what you posted for a while. I did a little rearranging of code (still what to do some more) but here is some results in about 23000 hits there was 14 missed pairs and 37 times the GPS data was not printed. The thread caught it but didn't make it print. There doesn't seem to be a problem with the IMU thread. Tried a mutex around serialprints in the main loop and put the gps print in the GPS thread but that did not seem to work - probably my fault. At this point its the challenge of getting it to work rather than just putting it in the main loop.

 

[defragster]

The delay I saw in the GPS data from IMU Filter of 500+ uS is not critical and without threading would be fine. It just gave me pause questioning if it was working right.

As noted this new scheme is less overhead than isr() Rx detect - but less accurate - rather than to near start bit ( plus nanosecs of interrupt overhead ) is can be 0-4 chars out of sync with chars in the FIFO not counted - but we might trigger the UART isr() to push those chars and that would eliminate that error - that needs to be done at some point anyhow and it a to memory transfer from CPU FIFO not a bus request with any delay.

Bottom line it looks like the TThread efforts are a distraction and getting this working right would be simpler and more linear without trying Tthreads yet. What it adds is just breaking up the math intense filtering code - that could be done with two threads - - thread ONE "just running filtering without care" and THREAD TWO for EVERYTHING ELSE - I/O and one pulling data and buffering it from IMU and GPS. Thread TWO would often be fast with no data to read or nothing to output - and Thread one would always be busy when there is fresh data.

In the end some of Thread TWO might split off with care and not impact the system - but I'm not sure in this quick hand waving what that would be to add any benefit worth the distraction of data protection from task switches during this early time.

 

[mjs513]

To be honest - I agree with you. Right now it has become an academic exercise, and spending way too much time on it. May continue to play with it on and off but going back to the unthreaded version is the better approach.

EDIT:

but we might trigger the UART isr() to push those chars and that would eliminate that error - that needs to be done at some point anyhow and it a to memory transfer from CPU FIFO not a bus request with any delay.

Ok, now you are beyond my knowledge to do that one. This has really become a learning exercise all around. Glad you joined in.

 

[defragster]

To be honest - I agree with you. Right now it has become an academic exercise, and spending way too much time on it. May continue to play with it on and off but going back to the unthreaded version is the better approach.

EDIT: Ok, now you are beyond my knowledge to do that one. This has really become a learning exercise all around. Glad you joined in.

Jiggering with the FIFO isr() Just a theory to me as well - but until there are enough chars in the FIFO ( Receive data above watermark UART_S1_RDRF ) they will sit until a default periodic timed transfer AFAIK. There is no public way to trigger that. I'll make a post to see if that is something that makes sense as a new public interface, and/or I can try a hack for that to see if it works as I expect. My code as above has run overnight and the average uS difference is 13 - but it ranges from -1 to 84 as I calculate it - meaning 4 characters are in the FIFO unaccounted for in that time calculation.

<edit>: My ham handed efforts to provoke the isr() didn't change the average or 3-4 char differences.

tThreads:: Academic exercise indeed - fun but the pitfalls are easier to get stuck in than to see in advance - and waste cycles working through or around . And while it has some ability to allow the IMU to run amuck and yet under control - beyond that - until that causes pain the rest of the data collection and processing looks easier without tThreads as this gets off the ground.

 

[mjs513]

Here is the core snip that adds the detection to readGPS() in the threaded case - easily changed for the non-Tthread case

Going to give this a try and see how it goes on the T3.5. Curiosity always gets me.

until that causes pain the rest of the data collection and processing looks easier without tThreads as this gets off the ground.

Agreed. Going to back to the non-threaded version and work with that version. Can't wait to see what Don comes up with. Going to go back to the uNavAHRS code again and see if I get the drift under control. But I doubt it.

 

[Don Kelly]

Want to run an implementation idea by the group...

I'm on travel for couple days, so limited ability to code. But I've got the Ver10 modified and ready to test with the simple 6-state Kalman and GPS. To start off, for test purposes, my thought is to just test AHRS, the Kalman propagation step, and the Kalman update step all at 5Hz.

But thinking about where we might head... what if we run AHRS at some fast rate (maybe 400Hz), smooth the accelNED values down to 20Hz and run the Kalman propagation steps at 20Hz, and then do the Kalman update steps (i.e., GPS measurement updates) at 5Hz?

This would allow pretty fast INS/GPS outputs for the user (20Hz) from the Kalman, and we'd be able to feed the Kalman smoothed 20Hz accelNED values which would be derived from the 400Hz AHRS outputs.

Anyway... wanted to throw that out there, since I know we're going to need some approach for getting smoother accelNED values for the Kalmna INS/GPS.

 

[mjs513]

I think building up to the max speed would be the approach to take. You get to see what breaks when . Think the faster you go the more noise you are going have to deal with from the IMU, just a guess here.

 

[defragster]

Don: have you posted the latest of your V10 code - or has it migrated to V11 since last posting?

Mike: Cool, going back to Don's code would be on a single path without mysteries from tThreads and focus efforts. That Snip may be enough to work with - or not. If you post a version of Don's latest with your merging of user values and #defines I could get that tested tonight. I didn't see a quick way to provoke the emptying of the FIFO - if I'm right about that being the source of the time error from uncounted bytes I am seeing.

 

[mjs513]

Hi Tim = working on it now. Give me about 30 minutes to test it out.

 

[mjs513]

Ok, I think I used the right version 10 that Don posted that I updated - but I don't think lat and long are updating correctly. Also there is no test if GPS data is ready. by the way if I try and change it, I start getting missed values. I have to think about this one some more.

 

[defragster]

You've got some time - coming up on dinner here so I'll be offline a couple hours if you get an update I'll take that one.
I posted my Serial timing dilemma ...