uNav INS

Cool, I wondered what TelephoneBill was looking at to get that that frequency.

There would be jitter from each interrupt to consider with more of them with the system already busy with IMU[int + i2c] + GPS_Serial. Not sure where the break even would be for improvement over the single 1 Hz and the added interrupts that would come with it and any overhead to keep the clock moving forward?

With a 1 second PPS from GPS it was better over time than RTC - but RTC only varied perhaps a hundred F_CPU cycles in an hour? I did a rolling average to smooth the jitter/cusps as a new count of cycles appeared between the 1 second interrupts to preserve the integrity of timing short term measurement to measurement - which it seems would be more important than even minute to minute.

Bumping the baud rate should not hurt - will keep the GPS data completion closer to IMU readings. I didn't want to push that past 460800 - but now seeing it has been tested.

Faster GPS updates seem interesting. I used the u-Center to get 10 Hz and it just doubled the same report every other one. I assumed it was pushing the envelope of 'space math' it could do? If it actually give more updates faster I wonder if they get less or more stable or consistent given the way my house moves ... just sitting here was it 3 to 12 inches/second I saw?


Don: I think you asked about F_BUS on T_3.6 - for 180 MHz it is in this same file "...\hardware\teensy\avr\cores\teensy3\kinetis.h" - just below the 240 MHz 'overclocking' for F_BUS about line 789::
Code:
#elif (F_CPU == 180000000)
 #define F_PLL 180000000
 #ifndef F_BUS
 // #define F_BUS 60000000
 #define F_BUS 90000000

I just bumped 180 MHz teensy to 90 Mhz F_BUS - it works - I don't see it changing much, still can't take i2c to 3.4 MHz like F_CPU==240 MHz. I did try 2.4 MHz and that works at both 60 and 90 F_BUS - bumps free Loop# passes about 4K.
 
This is useful sometimes.
Code:
void setup() {
  // put your setup code here, to run once:
  CPUspecs();
}

void loop() {
  // put your main code here, to run repeatedly:

}

// ***********************************************************************************************************
// *
// *                            CPU specs
// *
// ***********************************************************************************************************
void CPUspecs() {
  Serial.println();
#if defined(__MK20DX128__)
  Serial.println( "CPU is T_LC");
#elif defined(__MK20DX256__)
  Serial.println( "CPU is T_3.1/3.2");
#elif defined(__MKL26Z64__)
  Serial.println( "CPU is T_3.0");
#elif defined(__MK64FX512__)
  Serial.println( "CPU is T_3.5");
#elif defined(__MK66FX1M0__)
  Serial.println( "CPU is T_3.6");
#endif
  Serial.print( "F_CPU =");   Serial.println( F_CPU );
  Serial.print( "ARDUINO =");   Serial.println( ARDUINO );
  Serial.print( "F_PLL =");   Serial.println( F_PLL );
  Serial.print( "F_BUS =");   Serial.println( F_BUS );
  Serial.print( "F_MEM =");   Serial.println( F_MEM );
  Serial.print( "NVIC_NUM_INTERRUPTS =");   Serial.println( NVIC_NUM_INTERRUPTS );
  Serial.print( "DMA_NUM_CHANNELS =");   Serial.println( DMA_NUM_CHANNELS );
  Serial.print( "CORE_NUM_TOTAL_PINS =");   Serial.println( CORE_NUM_TOTAL_PINS );
  Serial.print( "CORE_NUM_DIGITAL =");   Serial.println( CORE_NUM_DIGITAL );
  Serial.print( "CORE_NUM_INTERRUPT =");   Serial.println( CORE_NUM_INTERRUPT );
  Serial.print( "CORE_NUM_ANALOG =");   Serial.println( CORE_NUM_ANALOG );
  Serial.print( "CORE_NUM_PWM =");   Serial.println( CORE_NUM_PWM );
  Serial.println("");
}
 
That is Very Handy - I posted during the K66/T_3.6 beta to keep track of what was reported versus the KS notes and to double check the compiled clocks as the board evolved. Didn't have the nice banner comment though.

I started to add at least F_BUS and F_CPU to the thread here for Perf/compile tracking but it wasn't as compact to extend as the cool "Serial.println("\n" __FILE__ " " __DATE__ " " __TIME__);" in here - I always pick up a Teensy and wonder what was on it and where source was from.

This is useful sometimes.
Code:
void setup() {
  // put your setup code here, to run once:
  CPUspecs();
}

void loop() {
  // put your main code here, to run repeatedly:

}

// ***********************************************************************************************************
// *
// *                            CPU specs
// *
// ***********************************************************************************************************
void CPUspecs() {
  Serial.println();

// ...
}
 
BTW: I wondered why I wasn't seeing 250 lines per second fly by for SRD==3, I had printRate at 25 and it resulted in :: -----Loop#:532262 IMU#:251

I recompiled with printRate at 1 and indeed then all 250 lines were displayed - this takes away 135K free loop counts :: -----Loop#:497259 IMU#:250

<edit>:
T_3.6 at 180 MHz printRate at 50 with SRD==0 and a few of the 1000Hz IMU updates might be missed - 240 Mhz could do it :: -----Loop#:41533 IMU#:999
> hitting 991 to 1001 IMU/sec with Max 20 TViewer updates/sec.

So printRate is working to skip prints and frees up more CPU time.

ALSO: Since I last compiled - and before recompile above - the time rollover happened at least once with no reported errors.

NOTE: I did get NaN's once when I was trying the F_BUS change noted prior - perhaps when I tried 45 MHz SPI with F_BUS==90 - it showed 'loss of IMU data' then it went NaN'ny.

Also the message 'loss of IMU data' - when it happens is a bit overzealous and uncontrolled on Serial.print()?
 
Last edited:
Mike, when does current clock wrap? I see "3225.952881" now. You said the time diff is tracked by the elapsedMicros - not the second counting time?

Here's the time progression that just went by::
4281.904297,
-13.012155
// ...
-0.004018,
0.046922,

It looks like a float runs out of room shortly after this showing 9 decimal digits? :: 16419.423828125 Not sure what the 'whole' limit is on float to have 8 good decimal digits without looking.

I'm wondering about keeping after the PartSec value in a float? The cycle counter wraps a uint32_t in under 19 seconds at 240 MHz. Resolving that only for a PPS/RTC second makes it safe - but how long do we need to keep seconds? Using uint32_t math makes the wrap around natural like micros for continuous defferences inside that safe 18 second window - which is what I started aiming at. The PartSec as shown is a natural for floating point - adds an extra multiple to get it to 100,000,000 range for uint32_t.
 
Geez... Guess I can't sleep anymore :) Everytime. Anyway while my eyes are still open:

Tim:
"when does current clock wrap? I see "3225.952881 now."
You said the time diff is tracked by the elapsedMicros - not the second counting time?" - I usually get it at around 4282 or so. That's with a T3.5 at 168Mhz. On a T3.6 at 240 or 180 might be sooner. dt which is using elapsedMicros is only in the filter (uNavIns) and is the time step for the filter.

The rtcTime that gets transmitted is calculated in the main loop only. It essentially a reference time. Float is really only good to about 6 decimals - you want more need to go to doubles. You might want to check these posts out:
https://forum.pjrc.com/threads/31233-float-and-double?highlight=float+accuracy and https://forum.pjrc.com/threads/48048-T35-issue-with-floats-int32s-conversion-maybe

Chris O. Great project you got going in post #250 and guess what I just got in the mail :) Another one for the todo list. In regards to post #252 that will come in real handy. Started printing some info on the CPU speed at the beginning of these sketches so when I plugged it in I can remember what the heck settings I used. :)
 
@ defragster
That is Very Handy-I posted during the K66/T_3.6 beta to keep track of what was reported versus the KS notes and to double check the compiled clocks as the board evolved. Didn't have the nice banner comment though.
Oh that was you thanks, yeah the banner was extremely hard to implement:cool:

Well here is one more from Frank B.
Code:
// ***********************************************************************************************************
// *(c) Frank B 2017/4 - License: MIT - A little function that is handy sometimes:
// *                            List Interrupts
// *
// *
// ***********************************************************************************************************
void listInterrupts() {
  unsigned adrFaultNMI = (unsigned)_VectorsRam[3];
  unsigned adrUnusedInt = (unsigned)_VectorsRam[IRQ_FTFL_COLLISION + 16];//IRQ_FTFL_COLLISION is normally unused
  unsigned adr;

  Serial.printf("F_BUS: %i\n", F_BUS); // check F_BUS speed
  Serial.printf("Systick priority: %i\n", SCB_SHPR3); // check Systick priority

  Serial.println("Interrupts in use:");
  Serial.println("NMI (non-maskable):");
  for (unsigned i = 1; i < 16; i++) {
    adr = (unsigned)_VectorsRam[i];
    if (adr != adrUnusedInt) {
      Serial.print(i);
      Serial.print(": \t");
      if (adr == adrFaultNMI) {
        Serial.print("Fault NMI");
      } else {
        Serial.print("\t");
      }
      Serial.print("\t0x");
      Serial.print(adr, HEX);
      Serial.println();
    }

  }
  Serial.println("IRQ:");
  for (unsigned i = 0; i < NVIC_NUM_INTERRUPTS; i++) {
    adr = (unsigned)_VectorsRam[i + 16];
    if (adr != adrUnusedInt) {
      Serial.print(i);
      Serial.print(": ");
      Serial.print("\tPriority:");
      Serial.print(NVIC_GET_PRIORITY(i));
      Serial.print("\t0x");
      Serial.print(adr, HEX);
      if (NVIC_IS_ENABLED(i)) Serial.print("\t is enabled");
      Serial.println();
    }
  }
  Serial.println("");
}

@ Mike
Code:
Great project you got going in post #250 and guess what I just got in the mail  Another one for the todo list
Just finished fixing the issue, I will probably release it today in the (A UBlox GPS Module Primer for beginners) thread.
LCD + touch = nice quick and easy configuration. :rolleyes:
 
Don Well - I using the Arduino IDE to load the sketches and tycommander as a sermon. It makes it a lot easier to see what going on.
 
Don:
Compiler is the Arduino IDE with TeensyInstaller toolchain, KurtE pointed me to SublimeText for External Editor [ has good UI/features and mandatory Folder wide Find ]
> allows multiple instances i.e.:: ...\uNavINS_MPU9250_I2C_MAGV4\ and ...\arduino_1.8.5_142\hardware\teensy\avr\ and ...

I prefer TyCommander Integrated to Arduino for programming:
> Upload calls out to TyComm to program and drops/connect Serial and Serial runs until reprogrammed
> Multiple Teensy connect and program by Serial# - Point to a Sketch on first compile to a unit and that Teensy then owns it until restart or Association dropped.
> Allows HALT with button press or GUI Bootloader button
> Allows RESET with GUI button

I prefer TyCommander for Serial Monitor - unless Integrated requires dropping 'Serial' to program:
> Fast and efficient and can open an instance ( Ctrl+N ) for each Teensy - or just Ctrl+Tab between each.
> Default buffer is 200,000 lines - can be made larger or smaller
> If you Scroll up from Bottom (mouse/keyboard) - it does not jump to allow review or cut and paste - unless at TOP during buffer filling
> Auto Scrolls when return to bottom
> RESET of Serial Data can be set to Persist or Clear across re-Program/reset, or Cleared at any time with Cltrl+Alt+X
> Can cut and paste Output - or use the disk copy where it is saved to a file.

NOTE: SPI_MST pair running overnight with SRD==0 and printRate==50, Slave running 20 TViewer updates/20 no problems - not a single "Bad CRC" in the 200K lines of buffer:
-----Loop#:41327 IMU#:994
-----Loop#:41773 IMU#:998
-----Loop#:41475 IMU#:996
-----Loop#:41501 IMU#:997
-----Loop#:41523 IMU#:997
-----Loop#:41586 IMU#:995
-----Loop#:41492 IMU#:996
-----Loop#:41495 IMU#:997
-----Loop#:41644 IMU#:996
-----Loop#:41534 IMU#:995
-----Loop#:41406 IMU#:998
-----Loop#:41292 IMU#:995
-----Loop#:41480 IMU#:997
-----Loop#:41194 IMU#:993
-----Loop#:41676 IMU#:1000
-----Loop#:41317 IMU#:997
 
NOTE: SPI_MST pair running overnight with SRD==0 and printRate==50, Slave running 20 TViewer updates/20 no problems - not a single "Bad CRC" in the 200K lines of buffer:
Best note of all. Considering the headaches I gave you all :)
 
...
"when does current clock wrap? I see "3225.952881 now."
You said the time diff is tracked by the elapsedMicros - not the second counting time?" - I usually get it at around 4282 or so. That's with a T3.5 at 168Mhz. On a T3.6 at 240 or 180 might be sooner. dt which is using elapsedMicros is only in the filter (uNavIns) and is the time step for the filter.

The rtcTime that gets transmitted is calculated in the main loop only. It essentially a reference time. Float is really only good to about 6 decimals - you want more need to go to doubles. You might want to check these posts out:
https://forum.pjrc.com/threads/31233-float-and-double?highlight=float+accuracy and https://forum.pjrc.com/threads/48048-T35-issue-with-floats-int32s-conversion-maybe

My Clock just happened to wrap as I was forming the post :)

That explains where I got stuck before - for Human/causal presentation Seconds+Decimal seconds is handy - for 'dt' usage only absolute time delta is needed.

The CycleCounter is a uint32_t just like micros() for compare and DIFF purposes, but is just a memory location to read with minimal overhead - but it runs at the F_CPU clocking rate so instead of 1/million it is 1/180M or 1/F_CPU - and can roll over in just over 18 seconds at 240 MHz. The CycleCounter runs no matter what, but is biased by crystal temp/behavior and can tend to be a couple hundred or thousand counts High or Low Per Second based on the processor and environment. So I saw it stabilized with a rolling average of observed Cycles/[PPS | RTC]

So those two uses can be divorced:
> Time Stamp is not critical to the Math Filtering but runs 'forever'
--- second update is on interrupt so it can change during READ operation
--- this must be why the GPS.nano value can be NEGATIVE on first of each second
--- This is part of why micros() has such overhead because it checks for next tick pending
--- This could be a paired value struct { uint32_t Seconds; float PartSec; }; - this would last for 49K days
> Sample logging for 'dt' must be absolute - but value only need to be current 'sample to sample'
--- This is what the PartSec() code I displayed before did and efficiently replaces micros() with more resolution
--- tracking this as a uint32_t would be lossless and consistent to the extent of cycle counter related to referenced Average of Cycles/second

Resolving those gets complicated by interrupts for the once per second [pps/rtc] update where things 'change' - so more interrupts means more cusps.
 
Tim
For dt I would leave it alone for kf iteration to iteration measurement (close enough).

Think Don is throwing a mismatch error between IMU and GPS if the delta is greater than 150 micros. So if we are within that tolerance it would be good. Which way is better? The simpler the better of the best ways.
 
Tim
For dt I would leave it alone for kf iteration to iteration measurement (close enough).

Think Don is throwing a mismatch error between IMU and GPS if the delta is greater than 150 micros. So if we are within that tolerance it would be good. Which way is better? The simpler the better of the best ways.

Addressing that best is to have accurate time recorded at point of data ready / transmission start: Both IMU and GPS_Serial_Rx - that is 'the interrupts' would say :: IMU_isr(){TimeNowIMU = PartSec()} or GPSSerialRx(){TimeNowGPS = PartSec()}

AFAIK :: The 'dt' needs to be relative to the sample at hand - knowing the time it covers since the last sample of the same type. The time when the iteration is performed is not the question - but how long the measured 'sample' appears to have been active. The iteration could be run a week after sampling - what is needed is a 'dt' between samples so they are properly weighted during the iteration math.

Given that the test for lost IMU would be Absolute and as simple as this, where TimeNowIMU should happen with an active IMU interrupt:
Code:
static uint32_t TimeLastIMU = 0; // must be preset/initialized to not trigger on startup
  if (uBloxData.numSV >= 4) {
    if ( TimeNowIMU == TimeLastIMU ) Serial.println(" WARNING: [B]DETECTED[/B] loss of IMU data !!");
    TimeLastIMU = TimeNowIMU;
    // ...
  }

The PartSec resolution would start with ~100 times the resolution and resolves some 6 times faster than micros. That may not improve the final results - but it will be stable beyond +/- 1 uS and less overhead. It doesn't stop the 1000 Hz millis interrupt - and does require adding a single 1 Hz interrupt - but the code there is not excessive.
 
Tim: Can't argue with your approach. Sounds reasonable. So if Don and no issues when not give it a try :)
 
I'm making slow progress... building a version using circular buffer to capture state data. may have it up and running tonight to test. I tend to make little changes at a time and re-compile and run. makes for faster debugging but perhaps slower development time...

Tim, looked at your last post and I'm think I follow it. Shall we give it a try?

Don
 
Update... Got a version working now. The states in the EKF are actually "error states" that get added to the "whole states" at each propagation and update stage. I'm seeing really really low values for the error states, so I need to go back and think about units and such. They seem too low...

EDIT: Thinking there are some unit errors in some of parameters perhaps, so am checking them to see if they make sense. For example, Lat and Lon are in radians yet the P for Lat is initiated to 10*10, which is probably intended to be 10 deg * 10 deg. I'm also going to change from plotting error states to plotting residuals, at least to start with.

Don
 
Last edited:
Mike and Don offline? No V5 to post tonight?

I should give the RTC version a try ... not sure if there is time in this day . . .
 
Tim,
Will release V5 within a few hours. I had pulled out error state statistics, but really need to pull out residuals data, so let me switch to that first.

We had heck of a hail storm last night, glad cars were in garage!

Don
 
Mike, Tim,
Here's a VERY preliminary version showing real-time monitoring of the GPS North position residual ( "y(0,0)" in the code). As I think I mentioned before, the residuals (and we have 9, 6 GPS + 3 magnetometer) are like the "heartbeat" of the EKF.

There are a number of ways to monitor the "heartbeat," one of which is to look at the raw value, mean, and sigma of the residual, and see where it lies within the bounds of +/- one-sigma R for that residual. So this is what I'm plotting.

Notice that when we monitor residuals, we're only doing it at the rate of the EKF Update step (because that's where residuals are calculated). Later, we may also want to monitor terms from the EKF Propagate step, which could be MUCH faster!

View attachment TVslaveDK.zip
View attachment uNavINS_CB_Ver2.zip
View attachment TV_Layout_CFVer1.txt
 
A couple of other notes wrt last post...

I'll add the code for the other 8 residuals next. Just wanted to post this preliminary code first. This is really cool being able to see the "heartbeat" of the EKF while it's running!!

What I think I'll do is show the 9 plots of for the residuals on the top 3/4 of the TViewer plot, and maybe a series of covariance (P) plots across the bottom. So then we can monitor the residuals performance, and also see how the filter "thinks" (via P) it's doing as well.

Once we get all 9 residuals plotting, we should be able to try different Q levels and start to get a feel for how adjusting Q affects performance.

Off to a couple of meetings, but will hopefully release a more complete version later.

Anyone have suggestions, thoughts, comments??

Don
 
here's an interesting thought...

once we get these residuals and covariance analysis plots up and running in real-time, what would be a way we might modify some of the Q and R parameters, in real-time? In other words, change Q and R on the fly and then be able to see the residuals and covariance results in real time...

The developer of TViewer was going to mod his code to allow for commanding, but he never go around to it. Could we do it with another serial port, and if so what's an efficient way to do it?
 
Well as an idea, just use the serial monitor to change the value. You can use a serial.available and test for the variable to change. Done that when I send commands to the rover. I would send them through the master so you can still see whats going in in Tviewer as you change the parameters.
 
Back
Top