uNav AHRS

Status
Not open for further replies.
Actually, right now the GPS is set to continually transmit the Long/lat solution at the rate specified (5hz, 10hz, etc.). In GPS terms nothing is sent from the GPS until you send a command requesting data to be transmitted. To do this all messages are turned off by default. The GPS_PORT_available is checking to see if the GPS data has been transmitted. Don't like it, but was putting it out there. I prefer the threads method that we are using.
 
Yes - also as I have it configured, the GPS is continually sending - and the Serial port is polled for arriving data emitted on the GPS schedule by the GPS with no prompting.
 
Yes - also as I have it configured, the GPS is continually sending - and the Serial port is polled for arriving data emitted on the GPS schedule by the GPS with no prompting.


Might be faster to check if (GPS_PORT.available) before calling this chain of events - as the call pushes the address - jumps and then calls - just to find out most often there is no data when only 500 bytes comes in per second.


Code:
bool UBLOX::read(gpsData *gpsData_ptr){  /* read the uBlox data */
// ...
 if(parse()){

bool UBLOX::parse(){ /* parse the uBlox data */
//...
    while ( _port->available() ) {

that is this should be cleaner:
Code:
if ( GPS_PORT.available && gps.read(&uBloxData) ) 
     ShowGPS();
threads.yield();
}
 
Last edited:
I took your suggestion and changed the GPS thread to read:
Code:
void readGPS(){
 while(1){  
    if(GPS_PORT.available() && gps.read(&uBloxData)) {
        showGPS();
      } else {
        hideGPS();
      }
   threads.yield();
 }
}

void showGPS()
{
    rtk.fixType = uBloxData.fixType;
    //rtk.numSV = uBloxData.numSV;
    //rtk.pDOP = uBloxData.pDOP;
    rtk.lat1 = uBloxData.lat;
    rtk.lon1 = uBloxData.lon;
    //rtk.heading = uBloxData.heading;
    //rtk.gSpeed = uBloxData.gSpeed;
    rtk.hMSL = uBloxData.hMSL;
    rtk.velN = uBloxData.velN;
    rtk.velE = uBloxData.velE;
    rtk.velD = uBloxData.velD;
    // Calculate utcTime for this loop iteration
    rtk.utcTime = (uBloxData.utcHour)*60.0f*60.0f +
            (uBloxData.utcMin) *60.0f +
            (uBloxData.utcSec) +
            (uBloxData.utcNano)/1000000000.0;
    rtk.upDated = true;
}

void hideGPS()
{
    rtk = {};
    rtk.upDated = false; 
}
If nothing else it does make it cleaner. I also added back in you GPS interrupt routine for time stamping the gps and IMU data printing it out. I attaching below.
 

Attachments

  • MPU9250_FiltersGPS_V2_threaded.zip
    31.7 KB · Views: 65
There was something that was bothering with the serialprintout function in the current implementation of what I last posted, i.e., lat and long would remain the same for long period of times. In printing them out separate from the function they would consistently be updated but would not in the function. I created a different function using printf which worked fine. Looks like there is a significant loss in precision when converting from double to float to string as required by Tviewer. This also could be the cause of apparent GPS dup values. The only other issue that is left is that I am loosing gps updates periodically. Have to figure that one out. Here is the updated code with the new function.
 

Attachments

  • MPU9250_FiltersGPS_V3_threaded.zip
    31.8 KB · Views: 77
Odd the GPS is missing? Quick debug to see if the Rx detect isr() is seeing that is below. Of course this counts on being able to see the output from loop() - which TViewer will hide if active. That would be a reason to use the DEBUG_PROXY teensy on Serial ( coutD.begin(coutD_BAUD); ) to allow messages like that to be sent at any time

Quick guess may be that the TThread is switching in middle of serialPortOut() and then returning only to complete with new data and then being called again with the same new data? Though with 200 ms between messages that would take some effort?

Possibility of Task Switch oddities like this are what make me question doing this up front as it will make the processing non-linear and side effects from unexpected task switch will cause distraction or confusion in interpreting the results?

What are the TIME units being set for the TThreads? I haven't looked enough to know what this means millis or micros?:
Code:
  threads.setTimeSlice(0, 1);
  threads.setTimeSlice(GPS, 150);
  threads.setTimeSlice(IMU, 150);
My thought is to set the times higher than expected { at least for the GPS } and then perform a thread.yield when complete to artificially assure when it has a task in hand it gets to complete. As for the IMU - that should be short to break up the computer intensive task to allow other things to process. Ideally ONLY one thread will write to a given Serial to avoid mixed messages, and that thread should also have either enough time to complete when there is a message queued to TViewer, or at least be sure that it completes the current task with desired data, and not data that is updated during a task switch.

Put this above setup:
Code:
elapsedMillis OneSec;
uint32_t RxGPS=0;

And at the end of setup():
Code:
    OneSec = 0;

And this in loop:
Code:
  if ( OneSec >=1000) {
    OneSec -= 1000;
    if ( 5 != RxGPS )
      cout.println("GPS under 5 Hz!");
    RxGPS = 0;
  }

And this in the GPS_serialrx_isr() change the inner loop to add the one line "RxGPS++;":
Code:
  if ( tt - xx > ((F_CPU / GPS_BAUD) * 30) ) {
    GPS_RX_time = tsGPS; // Start bit transition detected after 'idle' time
    GPS_newData=1;
    [B]RxGPS++;[/B]
  }
 
Quick update...I'm porting the Matlab code over to arduino in four chunks
- Coordinate transformations
- KF Initialization
- KF Propagation
- KF Update

I was working on the four coordinate transformations today, and then I noticed that Brian already had two of them (lla2ecef, ecef2ned) included in his uNavINS.cpp sketch. Wish I had noticed that earlier... So I'm regrouping to add the other two transformations (body2ned, ned2ecef) to his two. Slow going for me, am also having to waste... I mean spend... a bunch of time getting my 2017 tax stuff together. Anyway... making some progress.

Sounds like you two are making more headway on the IMU and GPS timing challenge!
 
Hi defragster,
Thanks for helping debug this. I incorporated the changes and sure enough I am getting messages that the GPS is under 5hz periodically, not consistently just periodically (no rhyme or reason that I can see). By the way, I did hook up a second monitor for the purposes of debugging from one of the serial lines connected to a converter but having problems with any terminal app able to see newlines. But I am not really using Tviewer right now for debugging this issue.

Thanks
Mike
 
Odd, I have not seen that on my end. I had code in that was counting and measuring the end of message timing from the Rx Start and it would have shown missing any. I didn't test what I posted - I have downloaded/extracted and I'll see about configuring your stuff to work to see what I see.

Perhaps it should be this in case they are wrapping across an arbitrary second:
Code:
    if ( 5 < RxGPS )
      cout.println("GPS over 5 Hz!");
    if ( 5 > RxGPS )
      cout.println("GPS under 5 Hz!");

As far as a Terminal App - use TyCommander :: forum.pjrc ... Teensy-Qt. It is good and reliable and with Tools / integrate to Arduino it replaces Teensy Loader very effectively allowing programming specific Teensy's when more than one is connected.
 
One thing I was reading was that if you have SBAS enabled it can impact the rate that data is sent. I turned it off and didn't get any more under 5hz messages. I updated with your changes just in case and didn't see it. EDIT: Spoke too soon - I am seeing periodically over and under 5hz updates. I think what is happening may be thread related. May have to use atomic variables or perhaps make them volatile's. Have to look a little more.

To avoid another post do you what version gcc we are using? Also if I wanted to add a compile flag -latomic, where would I add it?
 
I never looked at or edited anything SBAS related - so 'default' for me?

Indeed Threading - non-linear code - as noted made me wonder about starting TThreads too soon. Interrupt's are bad enough - but they are limited in function - ideally :) - but 'volatile'. And in my code I used only static to prevent stack data. Also marked as RAMFUNC to keep them in RAM versus a jittery delay to pull from flash to run, so they need to be compact. The T_3.6 Paul noted has a generous RAM space for that ( 32K? ) but much smaller/tiny on T_3.2 - and I don't recall correctly to give a number.

The default IDE 1.8.5 / TeensyDuino gcc 1.41toolchain in use.

For compile options look at ...\hardware\teensy\avr\boards.txt. There are Teensy specific sections: teensy35.name=Teensy 3.5 broken into menu option specific sections: teensy35.menu.opt.o3std=Fastest that control the options used during build. Putting the right flag on the right line should get it used if supported. It looks like it could be for example added to this line [ similar lines have added options] if using FASTEST compile: "teensy35.menu.opt.o3std.build.flags.optimize=-O3"
 
As a test I just pulled out GPS from the threading and just read the gps with the same function (I commented out the while loop of course :) ). It ran without an over/under messages that I could see. Once in long while I would get a skip but works like a charm the way I have it set up. I would like to get it working in the thread environment at some point but for now it seems like it will work. If I call it from the ISR will be even better.

Can't wait to see what you come up with. You are using things that I never have seen before (FASTRUN, RAMFUNC, have idea what they mean but where the heck are they defined? )

Was in the boards.txt file but wasn't sure what line to put it on, that for the advice on where to put the compile option. One of these days I have to switch over to the 3.6 for this :)

Mike
 
Odd the TThreading leads to skips. I'll have to power up what you posted and investigate. Can you tell me what the value (millis or micros?) you intended is: threads.setTimeSlice(GPS, 150);

Putting GPS solving into the GPS is contra-indicated.

As far as FASTRUN ( not ramfunc ) - that is just from staring at the forum too much for too long :) - it goes by in dribs and drabs . . . best explained in a single Paul post on interrupt Jitter - which I linked to in the WiKi thread.

Testing at lower speeds - or on T_3.5 - will keep this project honest so it doesn't get FAT and SLOW without seeing it. At 180 MHz now I'm probably seeing stuff that would have passed by at 240. Are you OC'ing the T_3.5 past 120?
 
Last edited:
Caught me a good time was about to pack it in. {just wrote too much here, rambling again}

Odd the TThreading leads to skips.
I think I know what the problem is but not quite sure out to fix it. The thread is updating faster than that data is being used. I tried a mutex but no luck - made it worse. Here is example of the data:
Code:
  0.185000, 13626.399414, 230.411453,    0.020, 3 40.77467968,-73.81463986,   27.5290    0.0630,   -0.0570,    0.0230   -1.1073,    1.9862,   63.3448   -0.0012,   -0.0019,   -0.0132
  0.204000,   0.000000, 230.631363,    0.020, 3  0.00000000,  0.00000000,   27.4880    0.0390,    0.0950,    0.0230   -1.2403,    2.1331,   62.9314    0.0021,    0.0380,   -0.0036
  0.185000, 13626.799805, 230.791290,    0.020, 3 40.77467898,-73.81464016,   27.4070    0.0570,   -0.0030,    0.0310   -0.6733,    1.7417,   63.4472    0.0099,   -0.0854,    0.0062
First column is a timestamp, second utcTime. The only time this line gets printed is when there is rtk.upDated=1. The only way, there are 0's for utc, lat, lon if rtk.updated=0.

Think the right way would be to use atomics which in of itself is not difficult but I can't seem to get an atomic to print. The way to do it would be to use atomic load() but there is no atomic library for those functions, only the header file to set the variables. That was why I was asking about -latomic. See this link: https://stackoverflow.com/questions/25311073/stdatomic-library-dependency-gcc-4-7-3. It does have a link to a libatomic.c file - now to figure out if I can use extern C to link to it.

I'll have to power up what you posted and investigate. Can you tell me what the value (millis or micros?) you intended is: threads.setTimeSlice(GPS, 150);
setTimeSlice is used for millis. If you look at the multithreading thread and the GitHub page there are more options. @tonton81 seems to got a good handle - he's helped explain mutex'es to me quite quite clearly.

As far as RAMFUNC - that is just from staring at the forum too much for too long - it goes by in dribs and drabs . . . best explained in a single Paul post on interrupt Jitter - which I linked to in the WiKi thread.
I'll have to take a look when I get a chance. I have more links to posts that are of interest I forgot what I saved them for :)

Testing at lower speeds - or on T_3.5 - will keep this project honest so it doesn't get FAT and SLOW without seeing it. At 180 MHz now I'm probably seeing stuff that would have passed by at 240. Are you OC'ing the T_3.5 past 120?
no OC'ing have it set at 120Mhz and "Faster" optimize.
 
I can't seem to get an atomic to print. The way to do it would be to use atomic load() but there is no atomic library for those functions,
As usual I am wrong, the gcc compiler is at 5.7.4 (forgot copy the exact number) and there is atomic library support. After awhile it finally sunk in that load/save and other functions are not supported for double - it will throw an error like I got. Its only does integral types, it does compile for float though. So if I want to use atomic double's I would have to convert to integer and then back to doubles which isn't horrible and easily done.

Cheers
 
Threaded V3 running. Seeing oddity - very confusing. Not sure what should be going on . . . too much flying by. Either change to IMU or GPS prints GPS - then a 2nd time again when imuDataRdy? That logic seems backwards - and even that would be duplicative as there is a single update function and it incorporates data from both at once without distinction?

Having loop() print the data and having readGPS()->hideGPS() zero the data makes a race condition if the GPS thread enters again before the print goes out - showing Zero Data.

Setup my 2 Mbps Serial1 for coutD and porting the GPS output there to make it separate from imu_Filter.

Put the T_3.6 at 120 MHz. Seeing alternate Under then Over for the 5 Hz GPS - suggests they would all be accounted for - just out of sync with the second cutoff, removed that - because it isn't accurate - see below.

Next step was removing hideGPS() from the readGPS() thread and putting it after the loop() finished the serialDataOut()

ONLY 80% :: A running counter in the Rx_isr() matches one in the serialDataOut() - it fails about 1 in 5!

Scanned Github readme and examples and I don't see the definition of this: threads.setTimeSlice(0, 1);
> But removing it stops regular cycling of the threads?
- Apparently it has to do with loop()?
-> Changed it to :: threads.setTimeSlice(0, 10);
-> Added threads.yield(); to the end of loop()

FIXED!!!! The isr() on GPS Serial Rx hit count now matches 1 for 1 with the PRINTS of the serialDataOut(); This shows the counts for the run since I made those changes:
2729.00==2729 | 0.170000, 33338.199219, 545.920227, 545.920166,
The left number is a FLOAT because I overloaded and used this rtk.heading value that wasn't being printed rather than add another global and to show integrity of the rtk data:: rtk.heading = GPSshows;

Here is my HACKED version of your code that is running with all the added anomalies ( my hacks) noted above in place - good luck and good night.
View attachment MPU9250_FiltersGPS_V3_threaded (2).zip
 
Step 1 (coordinate transformations library) done. I ended up just adding on several to Brian's previously built ones. They all compile fine, I will need to circle back and add some test cases to double-check the outputs.

View attachment cTransTest.zip

Next steps should be bit easier, create KFinit.h, then kfpropagate.h, then kfupdate.h from my Matlab code. Then I'll be ready to insert it back into Ver10 (or whatever Threading version you guys would prefer!)

Don
 
Hi Don,

For now I would stick with Ver10. Threaded version still needs debugging, it should be easy enough to incorporate - had a lot of experience doing that :)

Mike
 
CORRECTION: FASTRUN ( not ramfunc ) - is the way to force code to RAM for jitter free execution [ more so on T_3.5 and T_3.6 that have the extra RAM for execution ]

I started a scan of the TThreads thread since the README let me down and found this that confirmed the change I made to Thread ID Zero above that was needing more time in loop to complete the needed prints.
@mjs513

The default timeslice is WAY too large. Setting all thread timeslices to 1 as opposed to the default 100 results in about 1% overhead. Make sure to include thread 0 (which executes loop()): "threads.setTimeSlice(0, 1);".

Using a different timer with 100us ticks (the systick timer used by default has 1000us ticks), results in about 3% overhead with 1 tick timeslices.

It also suggests the cost of task switching is 1% and with sub millis timeslices perhaps 3% would be lost.

Since I was seeing current filter timing taking <800 micros [ at 180 MHz ] - timeslices of 1 ms would be a good start to allow it to complete currently [ though .yield() does properly forfeit time ], or chop it up when it takes longer - but not hold the system up for completing the GPS string reading. Ideally perhaps going to sub-micros timing would avoid the Filter processing from holding up the GPS data as I was seeing it after waiting 2.2ms for the serial transfer. Now the 460 Kbps has been seen to work - I could try doubling that to halve the transfer time - but default 64 byte buffer could overflow if not serviced each 1 ms.

The processing of the GPS uBlox string looks to be very fast [ O(n) ] - though I could time that - I assume overall it should take under 1 ms for total processing 100 bytes after pulling from the cpu's FIFO.

Update - the V3_Threads running since last night the Rx_isr() on GPS has now matched the number of processed strings each time for this many times:: 169208.00==169208
 
See we are back at it. Just went through the other thread since I've was on that one a lot. Anyway, I changed the timeslices to 1ms and things got significantly better - I implemented atomics (avoid locking) and running a test now but so far 2575.00==2585, ! ! ! ! ! ! ! !! ! ! !! !! ! ! ! !!!! ! ! ! PairMiss6. When I ran the modified code you posted was getting a missed pair a lot more often. This is typical now: 4080.00==4081

EDIT: By the way I updated the sketch to have the following defines. May make life easier.
Code:
#define mjs
//#define defragster

#if defined(mjs)
  // IMU Declares
  #define IMU_BUS       Wire  //Wire // SPI
  #define IMU_ADDR     0x68  //0x69 // 0x68 // SPI 9
  #define IMU_SCL       255  //47 // 0x255
  #define IMU_SDA       255  //48 // 0x255
  #define IMU_SPD    400000  //1000000 // 0==NULL or other
  #define IMU_INT_PIN    14  //50 // 1 // 14
  // Serial Declares
  #define cout       Serial
  #define cout_BAUD  115200
  #define coutD     Serial4  //Serial4  // Serial2
  //---- GPS Declares
  #define coutD_BAUD 2000000  // coutD for DEBUG port
  #define GPS_Start       1  // 1 // UBLOX gps(gps_Start);
  #define GPS_PORT  Serial1  // Serial1
  #define GPS_BAUD   460800
  #define GPS_SRX        27  //27 //9 // Must be set to Serial# Rx Pin #
  #define GPS_AltRx      27  // 27
#elif defined(defragster)
  // IMU Declares
  #define IMU_BUS       Wire  //Wire // SPI
  #define IMU_ADDR     0x69  //0x69 // 0x68 // SPI 9
  #define IMU_SCL       47  //47 // 0x255
  #define IMU_SDA       48  //48 // 0x255
  #define IMU_SPD    400000  //1000000 // 0==NULL or other
  #define IMU_INT_PIN    50  //50 // 1 // 14
  // Serial Declares
  #define cout       Serial
  #define cout_BAUD  115200
  #define coutD     Serial1  //Serial4  // Serial2
  #define coutD_BAUD 2000000 // 115200  // coutD for DEBUG port
  // GPS Declares
  #define GPS_Start       2  // 1 // UBLOX gps(gps_Start);
  #define GPS_PORT  Serial2  // Serial1
  #define GPS_BAUD   460800
  #define GPS_SRX        9  //27 //9 // Must be set to Serial# Rx Pin #
  #define GPS_AltRx      255  // 27 // else 255
#endif[CODE]
 
Last edited:
See we are back at it. Just went through the other thread since I've was on that one a lot. Anyway, I changed the timeslices to 1ms and things got significantly better - I implemented atomics (avoid locking) and running a test now but so far 2575.00==2585, ! ! ! ! ! ! ! !! ! ! !! !! ! ! ! !!!! ! ! ! PairMiss6. When I ran the modified code you posted was getting a missed pair a lot more often. This is typical now: 4080.00==4081

Ick - there should be no PairMiss - after the first "PairMiss0" where I sync the numbers because one starts before the other - ( the Rx isr() is running during setup of the imu ). { not sure if you saw how I did that - anytime the difference is greater than 10 I print the alert and re-sync the numbers }

As below I have "2620.00==2620" with no new "PairMiss" { T_3.6 at 180 MHz }. Just started a recompile T_3.6 at 96 MHz to confirm - so far no "PairMiss" at "2000.00==2000".

Seems like you soldered up your Serial PROXY Teensy :)

Just did a restart with 1ms threads - except 10 on the Zero thread for loop() to make sure the prints complete. Having the .yield() at the loop() end avoids it wasting time when there is nothing to do or the task is done.
Code:
threads.setTimeSlice(0, 10);
  threads.setTimeSlice(GPS, 1);
  threads.setTimeSlice(IMU, 1);
So far

Also - GPS and IMU for thread ID's scares me for a "C" naming conflict { like "struct GPS rtk;" }. Perhaps something like thGPSid and thIMUid

Other than the PRINTS - I haven't looked at what loop() code does - but putting that as a separate loopThread() would be more consistent with TThreads - though it takes more time as written. Having MORE TIME assures it will be 'atomic' for now.
 
Didn't have time to solder one up, so I took the lazy way out, been busy painting. On breaks doing this :). No problem with changing the thread ID will take care of. Wonder if its the difference in the speeds we are using. I am playing around with that now.

Other than the PRINTS - I haven't looked at what loop() code does - but putting that as a separate loopThread() would be more consistent with TThreads
You have to be careful with printing from the threads - take a look on the other thread and you will see a couple of discussion about putting mutex locks on the threads so there is no conflict.
 
From the other thread ( and tonton feedback) - and what I see - loop() IS just a thread - handled specially! That is why it needs a SetTimeslice! It will get task switched away just like any other thread!

That is also why when I increased the time to 10 ms for (0,1) (that is the loop timeslice ) the PairMiss went away on my end.

As far as SPEED - my T_3.6 is now compiled at 96 MHz and NO PairMiss and showing this :: 11700.00==11700 | 0.207000, 75122.601562, 2340.075684, 2340.076172,

I am seeing some few SerMon (TyComm) print anomalies - one field seems to be dropping and shifting the output - maybe it needs over 10 ms at times? Going to restart at 96 MHz and a 12ms loop() timeslice to see if that goes away.
{ Note these print anomalies are on the Serial1 PROXY debug output at 2 Mbps - so I may be seeing serial port overrun? }

<EDIT:> Recompiled 96 MHz with 12 ms for loop() slice and I see this as one set of 3 lines:
3522.00==3522 | 0.208000, 75945.601562, 704.555725, 704.557312, 0.010,3 48.21428056,-122.45051573, 43.7670 -0.3080, -0.0030, -0.2210
, 0.010,3 48.21428056,-122.45051573, 43.7670 -0.3080, -0, 0.010,3 48.21428086,-122.45051583, 43.7900 -0.2910, -0.0300, -0.2210
3524.00==3524 | 0.208000, 75946.000000, 704.956848, 704.956848, 0.010,3 48.21428226,-122.45051343, 43.8170 -0.2390, 0.0390, -0.2200
 
Last edited:
are you printing or using serial in two different threads? if so, a mutex will fix serial issue

i use the root console adb on pioneer NEX radio for serial access between threads, for capturing manually input radio time/date and dumpsys hdradio/callerid info for guages
 
are you printing or using serial in two different threads? if so, a mutex will fix serial issue

i use the root console adb on pioneer NEX radio for serial access between threads, for capturing manually input radio time/date and dumpsys hdradio/callerid info for guages

No - all the prints come from one function in one thread.

The next anomaly I see is almost 2,000 lines away:
5460.00==5460 | 0.208000, 76333.203125, 1092.151978, 1092.145264, 0.010,3 48.21430896,-122.45055183, 42.9650 -0.1890, -0.1090, 0.0290
5461.00==5461 | 0.209000, 76333.398438, 1092.352783, 1092.35515461.00==5461 | 0.209000, 76333.398438, 1092.352783, 1092.3551-0.0450, 0.0410
5462.00==5462 | 0.208000, 76333.601562, 1092.552856, 1092.554932, 0.010,3 48.21430856,-122.45055203, 42.7090 -0.2000, -0.1010, 0.0750

then again over 2,000 away [then again @9645, 10260]:
7943.00==7943 | 0.213000, 76829.796875, 1588.742310, 1588.742065, 0.010,3 48.21441086,-122.45051643, 49.4750 0.4220, 0.0160, 0.1180
65, 0.010,3 48.21441086,-122.45051643, 49.4750 0.4220, 72, 0.010,3 48.21441126,-122.45051753, 49.3740 0.3690, -0.0700, 0.1260
7945.00==7945 | 0.211000, 76830.203125, 1589.144287, 1589.141602, 0.010,3 48.21441266,-122.45051643, 49.4770 0.3740, 0.0560, 0.1030

I'll - fire up the IDE SerMon instead of TyComm and see if anything changes. IDE SerMon just showed a similar anomaly [11505 and 14166].

My PROXY is the 96 MHz T_3.0 I had handy - perhaps it is having an issue reading Serial1 and passing to Serial at 2 Mbaud? The lines are about 149 characters long.
 
Last edited:
Status
Not open for further replies.
Back
Top