uNav AHRS

[defragster]

Good to hear it makes sense - not always the case looking at something new. TThreads could eliminate some time wasted polling and assure all the filter/integration is done as often as needed without putting off the needed time for I/O. But with the IMU interrupt that isn't a problem. And given the GPS isr() code seems to work we could use that to work around excess polling as well without using TThreads. We could increase the Rx buffer to 128 bytes and call that a fixed time after StartBit based on baud rate and read the whole message at once and parse it.

I found the GpsImu.txt ... 7zip dropped it in 'current' folder - not the sketch subfolder where I looked - so I had the updated sketch for the edits posted above.

I'm happy I found Teensy! I started with some lesser 8 bit AVR's and no regrets since moving to PJRC. As you can see from my #posts I've been sort of neurotic about it - the PJRC quality and forum has made it a great way to get back into some coding.

 

[mjs513]

And given the GPS isr() code seems to work we could use that to work around excess polling as well without using TThreads.

You are probably right about that for this application and make it more compatible with non-Teensies.

the PJRC quality and forum has made it a great way to get back into some coding

I think the Teensy forum is great compared to others that I have seen. It's been a great learning experience. I always remember this quote - there are no dumb questions, just dumb answers. Everyone has been great in answering my sometimes dumb questions in areas I am really just dipping my toes into. The Teensy environment seems so much easier to resolve major issues and implementations with the core, etc. Paul is great, but I think he needs to be cloned a few times. I am rambling, sorry about that.

Anyway, going to give the GPS interrupt a try in a while. Thanks again.
Mike

 

[Don Kelly]

Am still making progress with my coding, taking longer than i thought with other stuff going on. Am tweaking the TelemetryViewer dispay now with the additional parameters, then will add in code for the real-time setting of Q values, then will post the code and photos... need another day or so... really looking forward to getting this piece done so i can see what y’all (that’s Texas talk) have been doing with the threading and gps...
Don

 

[mjs513]

Morning y’all. I reworked the threaded version that I posted earlier with @defragster's comments. I also broke sections of sketch out into their own tabs. Just for grouping and keeping the main sketch clean. Here is the final version for now.

 

[Don Kelly]

Here's an interim release of what I've been working on. This version does NOT have the Q updating capability yet, but it has these changes:

1) I added the QuickStats library, and use it to calculate mean and std of the innovations
2) Added 10 more statistics parameters to the serial port output
3) Changed the TelemetryViewer plotting to show innovation statistics (innovations, mean +-3 sigma bounds) plots on right side, and states, covariance, Q and R on left side.

Think what I'll do now is take a look at Mike's latest code so I can catch up. I suspect my version of the AHRS cpp and h may be one iteration behind now, so I want to take a look at that.

What I'd like to do with this TelemetryViewer stuff next is to go ahead and insert the ability to change Q on the fly (I tested it, just need to formally insert it). It would be nice to turn this TelemetryViewer interface code into some sort of library or tab so I could easily move it to the next version of AHRS, but haven't thought that through yet.

 

[mjs513]

Hi Don. Just gave your changes a try. Pretty neat being able to see changes as you rotate or fly the board around. Should really help you with tuning. Now you just have to figure out how to set it up for auto tuning

 

[Don Kelly]

thanks Mike, it's very cool to see the behavior of the innovations in real-time, very insightful. auto tuning (via system identification) would be a really cool R&D project. I would be interested in doing some of that, but first am most interested in getting this version (or quaternion version) stable, and then the GPS integrated. THAT will be slick. Then I'd love to come back to some auto-tuning!

If I let my version run for a couple hours, it seems like pitch and roll (and sometime yaw and heading) will be ~180 off when I check back. So I'm guessing I either have something missing from my version (i.e., not matched to the latest cpp and h files) or there's still some bugs in the mod180 or mod360 routines. Is Brian's last zip-file posting the latest version for the cpp and h files?

I looked through the threading code you posted Mike. looks really fascinating. for this app what's the advantage of doing the threading? i'm guessing there are some cool advantages, and I had been wanting to mess around with threading for some time!

Tim and I have talked at length about timing of the IMU measurements, and he's tried some really interesting ideas out. GPS measurements (position, velocity) are very accurately (nanosecond range I believe) time-tagged, but a concern of mine has always been how do we get super accurate time-tags for the IMU measurements? If we sample the IMU immediately after a GPS update, then I suppose we can infer the time is accurate, but I'm still not sure how really accurate that makes them. What are y'all's thoughts? Does threading make it easier somehow to get accurate time-tags?

 

[mjs513]

If I let my version run for a couple hours, it seems like pitch and roll (and sometime yaw and heading) will be ~180 off when I check back.

Hmm. I will have to let my version run and see what happens just for confirmation.

for this app what's the advantage of doing the threading? i'm guessing there are some cool advantages,

For this app probably little impact unless timing is improved. For a couple of my applications where I actually have multiple loops for different modes of operation having sensor (gps, wheel encoders, distance sensors, etc) all available in the different loops there is an advantage. Most of my hobby work revolves around autonomous rovers. I posted one them on the forum, https://forum.pjrc.com/threads/41117-Teensy-3-5-Autonomous-Robot?highlight=rover and https://forum.pjrc.com/threads/4574...enMV-Camera-(Machine-Vision)?highlight=openmv. Modes of operation are typically manual, roving with obstacle avoidance, waypoint navigation, semi-auto control with odometry, rc control, etc. Each major function gets called from the main loop and runs in there own loop until I tell it otherwise .

Tim and I have talked at length about timing of the IMU measurements, and he's tried some really interesting ideas out. GPS measurements (position, velocity) are very accurately (nanosecond range I believe) time-tagged, but a concern of mine has always been how do we get super accurate time-tags for the IMU measurements?

Haven't done any timing tests but the question I have how accurate between the time-tags is acceptable. Really just experimenting now.

Trying to work on a magnetic distortion routine but getting confused with some of the things they are saying in the paper. Will eventually figure it out. Its like they skip some steps, or when you were in college and the professor say a derivation was intuitively obvious

UPDATE: Almost forgot I am using a USB-FTDI breakout board to avoid using two teensy's right now.

 

[Don Kelly]

Is the FTDI board so that you can power the uBlox with 5V? I have been doing that or connecting a second USB to the uBlox to get 5V. I tried tapping off 3.3V from the T3.6 to power the uBlox, but it wasn't enough. It kept saying "More power Scotty, I need more power!!"

 

[mjs513]

I am using a T3.5 and powering the Ublox off the 5v. No problem with power with the T3.5. I know there are differences between the two you might want to check the specs on the T3.6. I am using the FTDI just to connect up Serial4 to add a second port to the PC. Not even using the 5v power. Only gnd, RX and TX. It registers as the second Com port to the T3.5.

By the way saw the jump in yaw you mentioned. Yaw increased by 20 degrees with a corresponding drop in pitch and roll by about 10 degrees. You know you can scroll backwards by clicking in the window and then use the scroll wheel on the mouse.

 

[defragster]

I'm powering prior pictured uBlox from Teensy 3V3 output no issues. It has active antenna too, the prior Adafruit Ult was passive - but ran fine the same.

Don - I think Mike is being lame [ ] and using FTDI instead of second Teensy for debug serial stream. If so I wonder what speed FTDI maxes out at?

I could see the filter code being interrupted harmlessly to good effect with TThreads - it could just loop and run head down without a care and get paused/resumed between data updates. Of course that would be overkill and just eat up all the time given it. { If I learned right from onehorse that successive iterations over the same data refines the results even without new data introduced? }

But other than I don't see this bound on data acquisition polling, or long periods of output anywhere? The short read of 100 bytes from FIFO fed Serial buffer { default size is 64 so it could overflow, but made larger } every 200 ms is easy to keep up. And the IMU even at 1K Hz runs well too as noted down to 24 MHz where the interrupt notifies of data ready without wait to transfer 7 words (?). Both of those seem to be 'read only' [short i2c/SPI request message]- just wait for data notice. And the Serial Rx isr() time tags the start bit so notice is on hand that the data stream will be ending in ~2ms.

Certainly straight forward enough to develop Arduino one thread and consider TThreads later.

Mike - my note about GPS.read() might not work 'ideally' as noted for TThreads - it returns TRUE on complete otherwise FALSE whether it was mid message or between messages or just got an error. Note sure if uBlox change would be called for to differentiate such cases - but again the Rx isr() lets us know when data is coming so it may not be needed to ask for a change, or get any value in that.

 

[mjs513]

I think Mike is being lame [ ] and using FTDI instead of second Teensy for debug serial stream. If so I wonder what speed FTDI maxes out at?

Yes I am. Besides all my Teensies are in use and don't feel like soldering up another one . Thought about it though. According to the FTDI Knowledgebase "The maximum Baud rate achieveable with FTDI's current devices is 3M Baud." Probably dependent on driver being used. There is an app note on it though.

my note about GPS.read() might not work 'ideally' as noted for TThreads

Right now I am using the isr flag as the test before I do the read. Other comments noted.

Don - just checked again the angles after a couple of hours of run time. Yaw went back to zero at some point but pitch went - 370???? roll stayed stable at -8 degrees. And no my board is not level so probably about right.

 

[brtaylor]

Strange, I just ran a super long test with my MPU-9250 on I2C and a 100 Hz IMU and filter update rate. For the first hour, everything seemed perfect. Was away for another 90 minutes or so to find the Euler angles completely messed up. I'm going to have to run another test; this time on a spare computer recording the whole input (IMU data) and output (Euler angles) data stream.

 

[brtaylor]

Doh! I think I figured it out - I'm not handling the micros() rollover correctly, used for computing dt. I should modify the code to use elapsedMicros(), which is rollover safe and better for computing durations. Until then, expect the filter to get messed up at a little over an hour from power up.

EDIT: Attached, updated code using elapsedMicros instead of micros(). Should now be fine running forever

 

[defragster]

I don't find elapsed in that zip but micros?

  if (!_initialized) {
    /* gather sensor statistics */
    if (!_timeLatch) {
      _startingTime = micros();
      _timeLatch = true;
    }
    if ((micros() - _startingTime) < _duration) {

In scanning the code ... that is only done when (!_initialized) ? Am I missing something? [ and should that be 'less than' ? ]

Usually as long as all the related vars are the same uint type - I see uint32_t in the .cpp and .h - the math should wrap okay. elapsedMicros deals with the same value - so unless used differently ... i.e. reset after use rather than tracked as a difference it will have the same 71.5 minute issue.

Is there value in this: I was looking at was running the RTC for a PPS and using that against the 120 to 240 MHz processor cycle count. Turning that to a microsecond equivalent has less overhead than the micros() calc as once started it is just a value read. It will vary with time and temp with the cpu freq - just like micros() - but tracked against RTC PPS ( ideally GPS PPS that most don't have as the pin is not brought out on uBlox ) I was thinking it might offer less variability over time and save a 1ms every few thousand calls with sub microsecond ( 1/120 to 1/240 ) specificity versus rounding on the uS. At 240 MHz it wraps just over 17.5 seconds - but 1 sec PPS can count up Ticks and zero the part seconds. I found this written into a elapsedMicros class - but in my testing I didn't go there.

Would that have value to finish off for use as a general timebase and for dt's like this?: _dt = (float)_t; // get the change in time
If so what form?

 

[brtaylor]

I don't find elapsed in that zip but micros?

  if (!_initialized) {
    /* gather sensor statistics */
    if (!_timeLatch) {
      _startingTime = micros();
      _timeLatch = true;
    }
    if ((micros() - _startingTime) < _duration) {

In scanning the code ... that is only done when (!_initialized) ? Am I missing something? [ and should that be 'less than' ? ]

That's a part of the code where I'm just gathering sensor statistics for one minute. I switched from micros to elapsedMicros on line 209 for the dt calculation (_t is defined as a type elapsedMicros in the header file). I used to do something like:

      _tnow = (float) micros()/1000000.0;
      _dt = _tnow - _tprev;
      _tprev = _tnow;

Usually as long as all the related vars are the same uint type - I see uint32_t in the .cpp and .h - the math should wrap okay. elapsedMicros deals with the same value - so unless used differently ... i.e. reset after use rather than tracked as a difference it will have the same 71.5 minute issue.

The issue with the way I was using it is that _tprev would be right before rollover and _tnow would be right after rollover. So my _dt calculation was giving a HUGE value and the wrong sign. The code integrates the gyros by _dt, which would cause the dynamics to move the sensor into a strange attitude.

Is there value in this: I was looking at was running the RTC for a PPS and using that against the 120 to 240 MHz processor cycle count. Turning that to a microsecond equivalent has less overhead than the micros() calc as once started it is just a value read. It will vary with time and temp with the cpu freq - just like micros() - but tracked against RTC PPS ( ideally GPS PPS that most don't have as the pin is not brought out on uBlox ) I was thinking it might offer less variability over time and save a 1ms every few thousand calls with sub microsecond ( 1/120 to 1/240 ) specificity versus rounding on the uS. At 240 MHz it wraps just over 17.5 seconds - but 1 sec PPS can count up Ticks and zero the part seconds. I found this written into a elapsedMicros class - but in my testing I didn't go there.

Would that have value to finish off for use as a general timebase and for dt's like this?: _dt = (float)_t; // get the change in time
If so what form?

That would be really neat! I like using micros (and have a uint64_t version coded in addition to the standard uint32_t), but I would guess that a millis version would also be useful.

 

[defragster]

Makes sense - missed the _t define - I only had the zip code on the machine I was on - and it should work Staying there for a minute makes sense during init - I knew I wasn't seeing it in full context. But not seeing other uses in those files I was lost.

Not keeping the types the same was the problem:

      _tnow = (float) micros()/1000000.0;
      _dt = _tnow - _tprev;
      _tprev = _tnow;

i.e. going through float. I was going to add a question about just tracking and storing everything as uint32_t and only converting to float when used? That would keep compares like that valid. But if that number is often read it would make sense to convert once for storage.

Not a millis version - but a cycle count version 1/F_CPU perhaps 1/240,000,000 or 1/120,000,000 on T_3.5 - so down to a few nanoseconds. Clock drift is +/- 300 cycles on the RTC over time & temp. Much less against a rolling average when using GPS PPS. Before I saw uBlox neglects the PPS I was counting on that - will roll back to commonly found RTC. It uses a temp sensitive crystal too - but less variable than the one in CPU ( I assume ). Those were some of the plot notes I added some time back. I won't be much help in the math - but thought having a more dependable clock could help keep the samples order and dt to a finer point. Also why I got the GPS Serial Start Bit trapped by an isr(). That won't be when the data was sampled - but a fixed offset after the calc started. And once the GPS starts sending ( if not sooner ) as noted before the IMU data should be buffered until the same time GPS data is ready to integrate - or the IMU data will be some 1 or 8 steps ahead ( depending on imu sample Hz ) of the GPS data when fed in to the part that uses it.

Back to the above point - I called my cycle count timer PartSec - it could return BOTH Integer and float? Here is my integer version:

uint32_t PPS_PartSec() { // 4J18
  return 1000000 * (((int32_t)ARM_DWT_CYCCNT - PPS_cCntPri) / (float)PPS_cAVG);
}

So returning float would save the multiply - but as you found that loses value of unsigned compare. And I did it to mere microseconds instead of pushing to 10's of nanoseconds? That math is much less than the code block to get microseconds. It is based on having a ( RTC/GPS ) PPS update keeping it relative to that second - so may need to return the PPS tracked Tick count too?

 

[brtaylor]

Which signal are you pulling from the uBlox for the timing?

One thing the lab at the University of Minnesota has been noticing is that it appears that the uBlox GPS is taking a couple of frames to compute it's position solution. So there's significant delay, even before counting the delay in transmitting the data from the receiver to the Teensy. Having a common time reference between the Teensy and the uBlox will make it much easier to work the Kalman filter.

I'm not sure if there's some way that we'll be able to get both on an absolute time reference together (i.e. both on UTC time) to get the total time delay directly, or if we'll need to estimate the time delay on the uBlox side and treat it as fixed, but having some way of keeping the uBlox and Teensy in sync should help immensely.

 

[defragster]

This post #206 has a photo - you can see the right end far red wire. I angled a female header there to pin holes - picked up the provided pins and one past. On that I hand soldered a wire to the + of the (tiny SMD) LED my unit has for PPS. Post #167 shows link to the $20 uBlox I got from N.J. - no longer in stock - and note it has a different but hopefully similar older gen Honeywell MAG.


I posted some numbers and collected stats - don't find it now - but over 55 hours the number of CPU cycles per PPS came and went - that was with PPS Pin on Adafruit Ultimate unit. Since I got the uBlox unit I haven't gotten back to that. But yes, with stable time and seeing how it relates to reported UTC time the from sensor readings to Start Bit timing should allow getting an offset and even predicting when it will arrive to start buffering IMU readings until parse is done - it seems the imu readings are spec'd from sample to presentation.

Don got the http://mrobotics.io/ unit you linked - it has exposed uBlox pin that could take a wire with good soldering. I got lucky with the SMD LED and a short wire tied to header - and my unit was one third the cost so less at risk.

 

[mjs513]

Brian

One thing the lab at the University of Minnesota has been noticing is that it appears that the uBlox GPS is taking a couple of frames to compute it's position solution.

You might want to check your navigation rate setting under rates in the configuration tab. According to the spec this is defined as:

The ratio between the number of measurements and the number of navigation solutions, e.g. 5 means five measurements for every navigation solution. Max. value is 127. (This parameter is ignored and the navRate is fixed to 1 in protocol versions less than 18)

You might want to lower the rate if you haven't already. Also as long as we are on settings you might want to adjust the Nav5 settings to your application.

PS Update: Got the beginnings of mag jamming corrections kind of working but not near ready for prime time.

 

[Don Kelly]

Brian,
I ran your new code and for me it blows up after a couple of iterations. Guessing it's something with _t or _dt. If I get a chance later on today will see if I can start back with the old timing code snippets and see if I can figure out what's up.
Don

 

[brtaylor]

Brian,
I ran your new code and for me it blows up after a couple of iterations. Guessing it's something with _t or _dt. If I get a chance later on today will see if I can start back with the old timing code snippets and see if I can figure out what's up.
Don

Shoot, I know what it is. I was just checking the timing output, which is correct, but I forgot to divide by 1000000 to put dt into seconds.

Not at a computer right now to fix it.

 

[Don Kelly]

That did it... it's back cycling again...

 

[Don Kelly]

Brian,
One thing that I see on the plots that looks a bit odd is that when I move roll and yaw, the three Q values (from both LQL' calculations) remain fairly consistent. But when I raise pitch, Qa(0,0) and Qh(0,0) drastically increase, whereas Qa(1,1) doesn't move much if at all. Seems odd that changes in pitch should change 2 of the 3 values so much, where changes in roll or yaw make almost no difference.

 

[Don Kelly]

Brian,
I ran the latest version all night, seems to have run fine.

What are your thoughts on next steps? Are you thinking about re-visiting the quaternion 4-State or 7-State and get that going, or using this Euler version and going ahead and integrating in GPS?