uNav INS
- Thread starter brtaylor
- Start date
defragster
Senior Member+
Ran overnight after the edit and restart for last edits.
The GSecs in Master output is GetSeconds - that is time based on the RTC clock ticks - the number since powering up and then the CycleCounter indicated portion of a second. So the RTC time sense is not tied to GPS return data - just like the rtcTime before it - it is just a way to serialize the TViewer output.
The Average Cycle counts looks to be in a tight range - maybe the clocking is more stable at 180 MHz - I was likely always running at 240 in months past. Of course Just Now - I do see these seconds where the last four average was: [179999632 then 179999650 then 179999627 then 179999625 then 179999625 then 179999606 ] in subsequent seconds.
The diff of avg/LOW and avg/HI above is 0.0000002444449 or -2.4444495234673794185853575718908e-7 - which seems pretty tiny at almost one quarter of a microsecond?
That is the widest spread I see from a quick scan of 7 hours - and for 1/4th of the numbers to swing that much it had to be changing by perhaps UP then DOWN 100 clock ticks? Before I had arrays to count the number and range of values to show the plots I presented before. Assuming the RTC is the more stable clock reference - the altered CPU Cycles Counts from CPU clock would impact micros() sense of time - and that is where I'm saying the 'dt' values would see F_CPU +/- 100 cycles second to second. The RTC is of course more variable that GPS_PPS - but by design it should be more stable than PLL Magic behind clocking the CPU from a slower crystal up to 180 MHz. Although RTC second feeds from an interrupt with some jitter.
The GSecs in Master output is GetSeconds - that is time based on the RTC clock ticks - the number since powering up and then the CycleCounter indicated portion of a second. So the RTC time sense is not tied to GPS return data - just like the rtcTime before it - it is just a way to serialize the TViewer output.
The Average Cycle counts looks to be in a tight range - maybe the clocking is more stable at 180 MHz - I was likely always running at 240 in months past. Of course Just Now - I do see these seconds where the last four average was: [179999632 then 179999650 then 179999627 then 179999625 then 179999625 then 179999606 ] in subsequent seconds.
The diff of avg/LOW and avg/HI above is 0.0000002444449 or -2.4444495234673794185853575718908e-7 - which seems pretty tiny at almost one quarter of a microsecond?
That is the widest spread I see from a quick scan of 7 hours - and for 1/4th of the numbers to swing that much it had to be changing by perhaps UP then DOWN 100 clock ticks? Before I had arrays to count the number and range of values to show the plots I presented before. Assuming the RTC is the more stable clock reference - the altered CPU Cycles Counts from CPU clock would impact micros() sense of time - and that is where I'm saying the 'dt' values would see F_CPU +/- 100 cycles second to second. The RTC is of course more variable that GPS_PPS - but by design it should be more stable than PLL Magic behind clocking the CPU from a slower crystal up to 180 MHz. Although RTC second feeds from an interrupt with some jitter.
Code:
[U]25035[/U].857422, 51.558754,157.131332,-130.056442, 61.444206, 48.214287,-122.450378, 44.043186, 0.995511, 0.617421, 0.309625, -0.309625, 1.000000, -1.000000, 1.004640, 0.593265, 0.234252, -0.234252, 1.000000, -1.000000, -0.772090, -0.003785, 0.671898, -0.671898, 3.000000, -3.000000, -0.055216, -0.004384, 0.133116, -0.133116, 0.050000, -0.050000, 0.040216, -0.000932, 0.080003, -0.080003, 0.050000, -0.050000, -0.001872, -0.009031, 0.019574, -0.019574, 0.050000, -0.050000, 0.000060, -0.000054, 0.001834, -0.001834, 0.020000, -0.020000, -0.001474, -0.000108, 0.003398, -0.003398, 0.020000, -0.020000, -0.001199, -0.000030, 0.002241, -0.002241, 0.020000, -0.020000, 0.165113, 0.165128, 0.287418, 0.049693, 0.049701, 0.049691, 0.039093, 0.034253, 0.043693, 0.087843, 0.089925, 0.093995, 0.008024, 0.007987, 0.008064,
[U]25036[/U].058594, 51.595146,157.052261,-129.967941, 61.532707, 48.214287,-122.450378, 44.056091, 0.996784, 0.649004, 0.294811, -0.294811, 1.000000, -1.000000, 0.973685, 0.623530, 0.218341, -0.218341, 1.000000, -1.000000, -0.741915, -0.079926, 0.610357, -0.610357, 3.000000, -3.000000, -0.034069, -0.011226, 0.128768, -0.128768, 0.050000, -0.050000, 0.027645, -0.000566, 0.080106, -0.080106, 0.050000, -0.050000, -0.001893, -0.007750, 0.018635, -0.018635, 0.050000, -0.050000, -0.001061, -0.000015, 0.001798, -0.001798, 0.020000, -0.020000, 0.001518, 0.000177, 0.003129, -0.003129, 0.020000, -0.020000, 0.002128, 0.000155, 0.002166, -0.002166, 0.020000, -0.020000, 0.165166, 0.165181, 0.287449, 0.049699, 0.049707, 0.049697, 0.039145, 0.034326, 0.043603, 0.087843, 0.089925, 0.093995, 0.008024, 0.007988, 0.008063,
[U]25036[/U].253906, 51.404800,157.183502,-129.656250, 61.844410, 48.214287,-122.450378, 44.066120, 0.958063, 0.676127, 0.281784, -0.281784, 1.000000, -1.000000, 0.961798, 0.649059, 0.208417, -0.208417, 1.000000, -1.000000, -0.623292, -0.144631, 0.551986, -0.551986, 3.000000, -3.000000, -0.082509, -0.017187, 0.127509, -0.127509, 0.050000, -0.050000, 0.061319, -0.000731, 0.079974, -0.079974, 0.050000, -0.050000, 0.013318, -0.006627, 0.018787, -0.018787, 0.050000, -0.050000, 0.002967, 0.000019, 0.001844, -0.001844, 0.020000, -0.020000, 0.000481, 0.000172, 0.003128, -0.003128, 0.020000, -0.020000, -0.002260, 0.000121, 0.002195, -0.002195, 0.020000, -0.020000, 0.165038, 0.165053, 0.287373, 0.049690, 0.049698, 0.049688, 0.039008, 0.034337, 0.043694, 0.087843, 0.089924, 0.093995, 0.008023, 0.007988, 0.008064,Tim,
You had asked about propFlag. I was using it to tell me when the EKF Update stage had been entered. I should have called it updateFlag, so I'll fix that. Sorry about the confusion. (EDIT: also it was called twice as you noted, so I fixed that too!)
The 77 parms I was pulling for TViewer were calculated/updated only in the EKF Update stage only (i.e., not in the Propagation stage).
Don
You had asked about propFlag. I was using it to tell me when the EKF Update stage had been entered. I should have called it updateFlag, so I'll fix that. Sorry about the confusion. (EDIT: also it was called twice as you noted, so I fixed that too!)
The 77 parms I was pulling for TViewer were calculated/updated only in the EKF Update stage only (i.e., not in the Propagation stage).
Don
Last edited:
defragster
Senior Member+
Oh yeah - this is a non-normal output version you were using. Just puzzling at 4 am when I finally noticed it. And odd the flag is set and NEVER referenced/used?
defragster
Senior Member+
Building on my last posted TV_Layout_CFVer8x.zip? You say Ver6?
Opps, I missed your Ver8. Will go back to your Ver8.
All I've been doing is trying to do a simple read of characters from the Master terminal. What I was doing is sending Lat Lon Alt to the Master terminal, and then looking for a 'h' input to halt the streaming and a 's' to start it back up. So really simple. But tried it out I get
ASSERT: NaN Fail index = 62
___ ASSERT FAILED ___ FILE, LINE#, Expression
/Users/donaldkelly/Desktop/uNavINS_CB_Ver6/C_serialPrint.ino
143
!std::isnan( MST_PrintVals[jj] )
___ ASSERT FAILED ___ STOPPING !!!!
P.S. I also was using the MPU9250 calibration routine (the one I modified and posted some time back) and verified that I was getting a solid consistent calibration. I repeated the cal a number of times, and for the most part, the values were very consistent if I did the steps in the same order and was still doing it.
All I've been doing is trying to do a simple read of characters from the Master terminal. What I was doing is sending Lat Lon Alt to the Master terminal, and then looking for a 'h' input to halt the streaming and a 's' to start it back up. So really simple. But tried it out I get
ASSERT: NaN Fail index = 62
___ ASSERT FAILED ___ FILE, LINE#, Expression
/Users/donaldkelly/Desktop/uNavINS_CB_Ver6/C_serialPrint.ino
143
!std::isnan( MST_PrintVals[jj] )
___ ASSERT FAILED ___ STOPPING !!!!
P.S. I also was using the MPU9250 calibration routine (the one I modified and posted some time back) and verified that I was getting a solid consistent calibration. I repeated the cal a number of times, and for the most part, the values were very consistent if I did the steps in the same order and was still doing it.
defragster
Senior Member+
#143 is a funny number? There are 77 values! [ so glad I had that print! ] in \C_serialPrint.ino
Should not go over 77. I made a mistake going to doubles in my code where I doubled that too and should not have, but I don't think I posted that version. Did you do your own conversion to DOUBLE print or use the code I posted to start?
<EDIT>:: Opps - naming error - the ZIP ( at 3:52 AM ) took the name of the TVIewer file not the INO and I didn't notice that. Please use a proper increment and name.
Code:
void serialPrint() {
int packetLength = 77;Should not go over 77. I made a mistake going to doubles in my code where I doubled that too and should not have, but I don't think I posted that version. Did you do your own conversion to DOUBLE print or use the code I posted to start?
Code:
for ( int jj = 1; jj < packetLength; jj++ ) {
if ( std::isnan( MST_PrintVals[jj] ) ) {
Serial.print("ASSERT: NaN Fail index = ");
Serial.println(jj);
assert_( !std::isnan( MST_PrintVals[jj] ) );
}
}<EDIT>:: Opps - naming error - the ZIP ( at 3:52 AM ) took the name of the TVIewer file not the INO and I didn't notice that. Please use a proper increment and name.
defragster
Senior Member+
Odd - not sure how it got to and past #78 The ZIP has Master and Slave in
As edited above :: Opps - naming error - the ZIP ( at 3:52 AM ) took the name of the TVIewer file not the INO and I didn't notice that. Please use a proper increment and name.
OK, I see that name issue now. Would be better perhaps to keep version numbers same for master, slave, and TViewer for clarity.
I'll see what's in that ZIP (TV_LayoutCFVer8x.zip) file and start from there. I made a uNavINS_CBVer5 but only thing I added from the old Ver4 was a tab to send some variables (LLA, PRY) to the Master terminal for displaying. This allowed me to view the Slave data (EKF parms) on my PC and the Master data (LLA, PRY) on my iMac. Which was pretty cool.
Don
I'll see what's in that ZIP (TV_LayoutCFVer8x.zip) file and start from there. I made a uNavINS_CBVer5 but only thing I added from the old Ver4 was a tab to send some variables (LLA, PRY) to the Master terminal for displaying. This allowed me to view the Slave data (EKF parms) on my PC and the Master data (LLA, PRY) on my iMac. Which was pretty cool.
Don
defragster
Senior Member+
Windows auto-names the ZIP using 'one file' - I didn't pay attention except to add the 'x' showing it is new and experimental. Internally is just all of the last 'uNavINS_CB_Ver4' I got from you - updated for FULL DOUBLE SPI_MST and restoring the lost SerialRX_isr and finally adding the background for 'cc' CycleCounter time as base on RTC ticks built into T_3.5 and T_3.6.
I'm about to progress and move the cc times around for dt usage and turn off the other values - ... maybe under #ifdef so before and after testing can be done and to make merging more obvious?
I'm about to progress and move the cc times around for dt usage and turn off the other values - ... maybe under #ifdef so before and after testing can be done and to make merging more obvious?
I've got the Ver4 you posted with the T-timeBase tab, and it's running well. Let's call that the "baseline" for now.
There's a few things I want to add, but think I'll break for today. Tomorrow I think I'll
- get propFlag renamed correctly
- try the routine to read char input from Master terminal in prep for adding ability to modify Q on the fly
Don
There's a few things I want to add, but think I'll break for today. Tomorrow I think I'll
- get propFlag renamed correctly
- try the routine to read char input from Master terminal in prep for adding ability to modify Q on the fly
Don
defragster
Senior Member+
Don:
If you've not changed anything I can put in the USB input code as posted to Stop and Start the TViewer output?
Then I can work in my other finish work and step back for you to continue?
If you've not changed anything I can put in the USB input code as posted to Stop and Start the TViewer output?
Then I can work in my other finish work and step back for you to continue?
Tim,
Perfect. I have not altered anything yet.
Here's what I had tried:
void readSerial(){
String charIn; //Declare a String variable to hold your name
//float newGPSPosnSigma;
//float newGPSVelSigma;
//float newMagSigma;
if (cout.available()!=0) {
charIn = cout.readString();
}
if (charIn == 's') { // start serialPrint2 printing
//cout.println("Read in s");
printToggleFlag = true;
}
if (charIn == 'h') { // halt serialPrint2 printing
//cout.println("Read in h");
printToggleFlag = false;
}
//newGPSPosnSigma=Serial.parseFloat(); //Method to read in float value
}
My idea, as a test case, was to have this flag (printToggleFlag) go true if the user entered s (start streaming) or false if user entered h (halt streaming). So the Master would stream LLA and PRY based on what user entered (s or h). Once this worked, I'd build in options to read in vales to charge R or Q parms.
That was my idea anyway, but guessing there are smarter ways to do it!
Don
Perfect. I have not altered anything yet.
Here's what I had tried:
void readSerial(){
String charIn; //Declare a String variable to hold your name
//float newGPSPosnSigma;
//float newGPSVelSigma;
//float newMagSigma;
if (cout.available()!=0) {
charIn = cout.readString();
}
if (charIn == 's') { // start serialPrint2 printing
//cout.println("Read in s");
printToggleFlag = true;
}
if (charIn == 'h') { // halt serialPrint2 printing
//cout.println("Read in h");
printToggleFlag = false;
}
//newGPSPosnSigma=Serial.parseFloat(); //Method to read in float value
}
My idea, as a test case, was to have this flag (printToggleFlag) go true if the user entered s (start streaming) or false if user entered h (halt streaming). So the Master would stream LLA and PRY based on what user entered (s or h). Once this worked, I'd build in options to read in vales to charge R or Q parms.
That was my idea anyway, but guessing there are smarter ways to do it!
Don
defragster
Senior Member+
Okay - will see what I can do adding a test before serialPrint based on user input. Probably a good way to parse - I only ever did a rudimentary thing as shown as it was what I needed. I saw this 'user debug console ' but a quick scan didn't show it as what I expected.
Here is a quick snap of current TViewer time stamps with GetSecond() - now up to 42407 and counting:
As well as the flag name - check the intended logic on the "propFlag = Filter.getekfPropFlag(); if (Filter.getekfPropFlag())" - not sure if the IF fires as expected - and it seems the IF should test the flag - not call again.
<edit>: Given the usage this is what I expected to see when the flag was set - with a single Filter call:
Here is a quick snap of current TViewer time stamps with GetSecond() - now up to 42407 and counting:
As well as the flag name - check the intended logic on the "propFlag = Filter.getekfPropFlag(); if (Filter.getekfPropFlag())" - not sure if the IF fires as expected - and it seems the IF should test the flag - not call again.
<edit>: Given the usage this is what I expected to see when the flag was set - with a single Filter call:
Code:
propFlag = Filter.getekfPropFlag();
if (propFlag) {
Last edited:
defragster
Senior Member+
Looking at the dt usage - that is just the time between entries to Filter.update() as it is used - not the time between individual readings? Pretty close for IMU - but will have some variation reading to reading - not relating to the data reading itself.
Though with clocked data sampled by interrupt it would be easier to just use 1000000/ (1000/ ( SRD+1 ) ) ? As long as samples are never missed.
That dt will not represent GPS data ? Maybe that is hardcoded later? Supposing it is treated with a dt the same way?
Just pondering keeping the sample weighting right - there is enough variability in the sample time and reading as the device presents it - having them treated fairly seems the only way to keep from introducing oddities. The GPS baud could double or triple it seems and that would almost eliminate IMU overlap - even at SRD==0
Also in that I see :: uBloxData.lat * D2R, uBloxData.lon * D2R - that math happens 50 or 500 times per second with newIMUData, but only changes 5 times per second - would be better to extend uBloxData.lat struct to have a double value stored for that as it comes in and calculated one time per update.
<edit>: I manually did this with static placeholders - it is not significant.
I'll go do the input parser and come back and not risk breaking anything in case I'm not seeing the whole usage picture.
Though with clocked data sampled by interrupt it would be easier to just use 1000000/ (1000/ ( SRD+1 ) ) ? As long as samples are never missed.
That dt will not represent GPS data ? Maybe that is hardcoded later? Supposing it is treated with a dt the same way?
Just pondering keeping the sample weighting right - there is enough variability in the sample time and reading as the device presents it - having them treated fairly seems the only way to keep from introducing oddities. The GPS baud could double or triple it seems and that would almost eliminate IMU overlap - even at SRD==0
Also in that I see :: uBloxData.lat * D2R, uBloxData.lon * D2R - that math happens 50 or 500 times per second with newIMUData, but only changes 5 times per second - would be better to extend uBloxData.lat struct to have a double value stored for that as it comes in and calculated one time per update.
<edit>: I manually did this with static placeholders - it is not significant.
I'll go do the input parser and come back and not risk breaking anything in case I'm not seeing the whole usage picture.
Last edited:
defragster
Senior Member+
Still printing this TViewer timestamp - no rollover to here: 62804.253906
I have the 's' and 'h' and '?' input working ... will get what I have cleaned of some cruft and zipped shortly.
Here is '?' Help::
TViewer starts and stops on command
<edit>: tested a 'p' for pause Master with delay(500) - seems harmless - under the 600 ms for missed IMU
>> HOWEVER :: 'pp' gives a 1 sec pause and that crashes the system not unlike Don saw earlier:
Except it failed on INDEX == #62 - The '143' comes from LINE NUMBER !!!! - DOH...
Scary that a skip in IMU data immediately crashes the system?
I have the 's' and 'h' and '?' input working ... will get what I have cleaned of some cruft and zipped shortly.
Here is '?' Help::
TViewer starts and stops on command
<edit>: tested a 'p' for pause Master with delay(500) - seems harmless - under the 600 ms for missed IMU
>> HOWEVER :: 'pp' gives a 1 sec pause and that crashes the system not unlike Don saw earlier:
Except it failed on INDEX == #62 - The '143' comes from LINE NUMBER !!!! - DOH...
Scary that a skip in IMU data immediately crashes the system?
Last edited:
defragster
Senior Member+
uNavINS_CB_Ver5
Starting code for uNavINS_CB_Ver5 attached:
Below is what is added for timing - most resides in T_timeBase.ino.
Also added serialEvent() to MAIN.ino on Serial that has to be manually called from loop() as void yield() defeats this by design
With extra debug this was running at "-----Loop#:453096" - cleaned that up and it went to "-----Loop#:486199" with single test for Serial.available
>> Removing if ( Serial.available ) raises it back to :: "-----Loop#:526498" : on T_3.6 yield() does this for 7 ports. // just for ref - not critical
This reports IMU loss if two GPS reads do not have an newIMUData processed :: if ( RTCvsIMU > 1 )
>> For some reason any pause in processing can generate
As used::
Starting code for uNavINS_CB_Ver5 attached:
Below is what is added for timing - most resides in T_timeBase.ino.
Also added serialEvent() to MAIN.ino on Serial that has to be manually called from loop() as void yield() defeats this by design
With extra debug this was running at "-----Loop#:453096" - cleaned that up and it went to "-----Loop#:486199" with single test for Serial.available
>> Removing if ( Serial.available ) raises it back to :: "-----Loop#:526498" : on T_3.6 yield() does this for 7 ports. // just for ref - not critical
This reports IMU loss if two GPS reads do not have an newIMUData processed :: if ( RTCvsIMU > 1 )
>> For some reason any pause in processing can generate
As used::
Code:
// Added these globals to MAIN.ino
volatile uint32_t ccTimeNow_IMU = 0;
volatile uint32_t ccTimeNow_GPS = 0;
volatile uint32_t cctAvg = F_CPU;
Circular_Buffer<uint32_t, 4> cctASec;
Code:
// in MAIN.ino - records time of incoming data ready interrupt
void runFilter() {
ccTimeNow_IMU = ARM_DWT_CYCCNT; // records time of IMU data ready for reference - or compare to prior to know time between
newIMUData = 1;
}
Code:
// in T_timebase.ino - catches start of GPS incoming serial data : setup and attach in in SetupPartsec()
void GPS_serialrx_isr() {
ccTimeNow_GPS = ARM_DWT_CYCCNT; // records time of GPS Tx begin for reference - or compare to prior to know time between
detachInterrupt(digitalPinToInterrupt(GPS_SRX)); // must re-attach interrupt on receive complete
}
Last edited:
Tim,
Reading through your posts now. Will have time later this morning to try out your mods!
As far as dt goes, if I understand your question, I think there are two general ways to successfully treat dt in these filters. The first is to hard-code the dt time and force the filter to do any propagation or update steps at that particular interval. The advantage there is that the PHI matrix becomes constant and thus calculations can be faster.
The alternative is to let dt float to whatever the interrupts provide, which is how Brian's original version is set up. The advantage to this approach is that the propagation step can be whatever dt tells it it is.
I'm thinking it's best that we keep dt floating for now. But an eventual user may decide for his/her application to fix dt for simplicity and compute time savings.
Don
Reading through your posts now. Will have time later this morning to try out your mods!
As far as dt goes, if I understand your question, I think there are two general ways to successfully treat dt in these filters. The first is to hard-code the dt time and force the filter to do any propagation or update steps at that particular interval. The advantage there is that the PHI matrix becomes constant and thus calculations can be faster.
The alternative is to let dt float to whatever the interrupts provide, which is how Brian's original version is set up. The advantage to this approach is that the propagation step can be whatever dt tells it it is.
I'm thinking it's best that we keep dt floating for now. But an eventual user may decide for his/her application to fix dt for simplicity and compute time savings.
Don
defragster
Senior Member+
Don,
Glad I didn't break anything. Though my changes are actually superficial - information is at hand - but only the TViewer RTC_timestamp was changed. And removed some micro() usage.
The dt could be set from two interrupts which was what I intended but did not actually switch.
RE dt - Given we are using constant time devices - each sample is from device clocking - trying that could not only more closely match the reality - but save a great deal on computation. Especially since we've bypassed the native FPU using double. Without that interpolating for changing dt would be cool if float resolution became useful. The Teensy has room for both sets of code - could do a runtime switch or run both?
Scary thing about interrupt is making sure the int doesn't hit while reading and start changing the numbers - so I didn't want to do that quick and have something odd show up.
But ref to these on this ccTimeNow_IMU and last sampled ccTimeNow_IMU for IMU and GPS would give best guess at dt - if not using the IMU's ( 1/ 1+srd ) or 200 ms for GPS.
and:
Glad I didn't break anything. Though my changes are actually superficial - information is at hand - but only the TViewer RTC_timestamp was changed. And removed some micro() usage.
The dt could be set from two interrupts which was what I intended but did not actually switch.
RE dt - Given we are using constant time devices - each sample is from device clocking - trying that could not only more closely match the reality - but save a great deal on computation. Especially since we've bypassed the native FPU using double. Without that interpolating for changing dt would be cool if float resolution became useful. The Teensy has room for both sets of code - could do a runtime switch or run both?
Scary thing about interrupt is making sure the int doesn't hit while reading and start changing the numbers - so I didn't want to do that quick and have something odd show up.
But ref to these on this ccTimeNow_IMU and last sampled ccTimeNow_IMU for IMU and GPS would give best guess at dt - if not using the IMU's ( 1/ 1+srd ) or 200 ms for GPS.
Code:
void runFilter() {
ccTimeNow_IMU = ARM_DWT_CYCCNT;and:
Code:
void GPS_serialrx_isr() {
ccTimeNow_GPS = ARM_DWT_CYCCNT;Tim, Mike,
Here's a new version, uNavINS_CBVer6, which includes the following minor mods to Tim's Ver5. I DID NOT change the TViewer text file or the TVSlaveDK5 file.
- I corrected the propFlag names to updateFlag (old name was confusing)
- I pulled out Tim's serialEvent code into a separate tab
- I cleaned up some of the code and spacings in all the tabs
- I'm streaming LLA and a few other parms to the Master terminal, while the Slave 77 parms remain the same
- I added a few commands to the ones Tim created:
- 'q' doubles the gyro and accel Q sigma values (by changing terms in the Rw diagonal elements)
- 'r' resets them back to the default levels (by changing Rw diagonals back to default levels)
I'm still wrestling with how best to allow user to modify Q and R levels, so the 'r' and 'q' levels commands I describe above are really just test code to see if I could get them to change in real time, which they do! So try typing in 'q' on the Master terminal and watch the jumps in the TViewer plots on the Slave side. Pretty neat. Then try 'r' and see the plots go back to earlier levels. So not sophisticated... really more of a first try!
Don
View attachment uNavINS_CB_Ver6.zip
Here's a new version, uNavINS_CBVer6, which includes the following minor mods to Tim's Ver5. I DID NOT change the TViewer text file or the TVSlaveDK5 file.
- I corrected the propFlag names to updateFlag (old name was confusing)
- I pulled out Tim's serialEvent code into a separate tab
- I cleaned up some of the code and spacings in all the tabs
- I'm streaming LLA and a few other parms to the Master terminal, while the Slave 77 parms remain the same
- I added a few commands to the ones Tim created:
- 'q' doubles the gyro and accel Q sigma values (by changing terms in the Rw diagonal elements)
- 'r' resets them back to the default levels (by changing Rw diagonals back to default levels)
I'm still wrestling with how best to allow user to modify Q and R levels, so the 'r' and 'q' levels commands I describe above are really just test code to see if I could get them to change in real time, which they do! So try typing in 'q' on the Master terminal and watch the jumps in the TViewer plots on the Slave side. Pretty neat. Then try 'r' and see the plots go back to earlier levels. So not sophisticated... really more of a first try!
Don
View attachment uNavINS_CB_Ver6.zip
Last edited:
defragster
Senior Member+
Don: The posted _Ver6 code compiled no trouble and has been running away just fine for 8.212 hours it seems.
Would be interesting to see the speed change with fixed dt math - If I read/recall the onehorse code correctly it assumed a fixed time value in the code loop? That may have been a different world of math - but I saw Delta as a hardcoded value.
If using an observed dt is to be the case - tracking the time between interrupts would be much more accurate that 'the time between filter re-entry' - if I read that right.
Completing the step and math to get the elapsed 10's of nanoseconds should be easy to implement with the samples in :: debugGPS( )
where:
Might work as:
I can test this and update the debugGPS() {after sleep} to do it if you don't first:
A FASTER native float value ( no *10000000 ) - add this version to T_timeBase.ino:
Would be interesting to see the speed change with fixed dt math - If I read/recall the onehorse code correctly it assumed a fixed time value in the code loop? That may have been a different world of math - but I saw Delta as a hardcoded value.
If using an observed dt is to be the case - tracking the time between interrupts would be much more accurate that 'the time between filter re-entry' - if I read that right.
Completing the step and math to get the elapsed 10's of nanoseconds should be easy to implement with the samples in :: debugGPS( )
where:
Code:
static uint32_t cctLast = 0;
// ...
coutD.print( ccPartSec( cctLast, ccNow ) );
cctLast = ccNow;Might work as:
Code:
static uint32_t cctLast_IMU = 0;
// ...
coutD.print( ccPartSec( cctLast_IMU, ccTimeNow_IMU ) );
cctLast_IMU = ccTimeNow_IMU;I can test this and update the debugGPS() {after sleep} to do it if you don't first:
A FASTER native float value ( no *10000000 ) - add this version to T_timeBase.ino:
Code:
float FccPartSec( uint32_t CntPri1, uint32_t CntPri2 ) { // 6A18
return ((CntPri2 - CntPri1) / (float)cctAvg);
}
Code:
static uint32_t cctLast_IMU = 0;
// ...
char textTemp[30];
dtostrf( FccPartSec( cctLast_IMU, ccTimeNow_IMU ), 10, 8, textTemp);
coutD.print( textTemp );
cctLast_IMU = ccTimeNow_IMU;