uNav AHRS

Status
Not open for further replies.
Not sure if you are going to see the same problem with the T3.6. I ran the i2c at 400Khz and it the IMU just hung no data after a few seconds. Same thing at 1000000 but it worked for a little then hung again. Onehorse mentioned that its a timer issue with i2c_t3 library. Next time I load the sketch up I will give 3.4M a try and see what happens.
 
Not sure if you are going to see the same problem with the T3.6. I ran the i2c at 400Khz and it the IMU just hung no data after a few seconds. Same thing at 1000000 but it worked for a little then hung again. Onehorse mentioned that its a timer issue with i2c_t3 library. Next time I load the sketch up I will give 3.4M a try and see what happens.

Odd - I never saw that normally of course i2c_t3 was a recent addition - I did when I pushed for higher rates over 400K with Wire - but not when I went to the Standard 3.4M as MAX - it keeps running with no hang.
 
Finally have some time to circle back to this, after some grant proposals and working to get a massive research aircraft ready to fly. I'm planning on folding in many of the different AHRS codes into this library / repo and cleaning up the library, documentation, and examples:
https://github.com/bolderflight/uNavAHRS

Idea being, having many similar classes (i.e. same declarations and methods, just different objects depending on what type of filter is desired). Really impressed (and slightly daunted) this grew to >30 pages...
 
Awesome. Will be glad to help with testing or whatever!

I've just got RTK running on the Beaglebone Black (BBB) with WiFi adapter (to pull in base station data). I'm having issues with WiFi stability, thinking it might be the BBB HDMI interfering with the WiFi, so will try to disable HDMI. Anyway, would be exciting to pull in RTK GPS accuracy with the AHRS filters, mixing in BBB linux with the Teensy!! Took me longer than I thought it would, but nice to see the RTK-Fix solution working on BBB. If I can get the WiFi a bit more stable, my plan is to fly the RTK BBB on my drone this week, connecting to the PixHawk2. But would be nice to replace the PixHawk2 with our own IMU and autopilot and GPS at some point...
 
Awesome. Will be glad to help with testing or whatever!

I've just got RTK running on the Beaglebone Black (BBB) with WiFi adapter (to pull in base station data). I'm having issues with WiFi stability, thinking it might be the BBB HDMI interfering with the WiFi, so will try to disable HDMI. Anyway, would be exciting to pull in RTK GPS accuracy with the AHRS filters, mixing in BBB linux with the Teensy!! Took me longer than I thought it would, but nice to see the RTK-Fix solution working on BBB. If I can get the WiFi a bit more stable, my plan is to fly the RTK BBB on my drone this week, connecting to the PixHawk2. But would be nice to replace the PixHawk2 with our own IMU and autopilot and GPS at some point...

That's great to hear! I've had issues with wifi stability as well, but this is with a dongle known to not work well (thanks to poor information when it the BBB was first released). Have you tried using external power instead of USB? Wifi needs high current and trying off the USB power could cause stability issues.

Funny you should mention the autopilot software, that's one of the big pushes I was working on. v1.x has been flying on simple aircraft at the University of Minnesota for a few months. Just got a bunch of kinks worked out getting a massively complicated research aircraft ready to fly. The uNav INS software has been flying for a few months as part of the v1.x software stack and seems to be working well. I think the gains may be a little different than what's posted, but the algorithm is good. I plan on working on incorporating all the different INS approaches into a common library once I'm done with the AHRS.

I recently started working on 2.x version of the software. The repo for the software will be located here, the documentation here, and it's built for this BBB and Teensy 3.6 hardware. It's a radically different approach than Pixhawk, drone code, or ROS, but I think in very good directions. The development is open source and I'd love if people would like to contribute. There's a lot of potential; it's being used at UMN for UAS, aeronautics, precision ag, high altitude balloon, and nanosatellite research and NASA will begin using the system for UAS and aeronautics research in a few months.
 
Very cool! As far as power, I am supplying the BBB with external 5V. But I'm using a USB hub to then power both the M8T GPS board and WiFi dongle, so maybe those two are not getting enough from the BBB USB port. Am currently using the smaller "Ourlink" USB adapter from Adafruit, but will be getting in their other USB adapter (with the longer higher gain antenna) this week.

I need to check out your software, sounds neat!!
 
Hi Brian.
Just got a bunch of kinks worked out getting a massively complicated research aircraft ready to fly.
Impressive, that's all I can say :) Can't wait to see a video of it flying around. Resisting the urge to ask a million questions about it. You brought out the engineer side me with that video.

The uNav INS software has been flying for a few months as part of the v1.x software stack and seems to be working well.
Hmm. Looks like it is all set up to run on a Teensy with no changes :) Wish I saw this before. Just broke down my test set up a few days ago. Will have to put it back together. I can actually give this a test outside in my rover now. I managed to get mavlink working sending IMU and GPS data back to APM planner and controlling the whole thing with a joystick. Posted the set up in General Discussion forum.

Tried using Mavesp8266 (sending mavlink data over a 8266) but during outside tests kept loosing the signal. Indoor bench testing no problem. Think it would have worked a whole lot better if I there was an external antenna on it. So I switched over to 3dr radio clones and it worked no problem. May go back to it later on and do a few more tests just because I am stubborn.


Mike
 
I checked and the gains in the uNavINS library are the same that we've been flying. Yes, we run the same code on the Teensy and BeagleBone Black at the moment; Teensy is setup to handle baseline navigation and flight control laws in addition to managing all of the sensors and actuators. BeagleBone Black runs all of our research software. So a lot of our code is written to run on either.
 
Nice Brian.

Been experimenting with CAN bus and one of the tests, since I couldn't resist, was sending IMU and GPS data over CAN. Works rather nicely. Basic setup was to have one Teensy collect the data and then the receiving Teensy to process the data. Was using tonton81's new IFCT library for the test: https://forum.pjrc.com/threads/5290...Teensy-Library?p=182567&viewfull=1#post182567 . Was also looking at UAVcan and CANaerospace but seems a bit overkill.
 
For the AHRS library, these are the algorithms and improvements that I'm seeing, please let me know if I'm missing any:
* BRT 4 state quaternion KF
* BRT 4 state quaternion EKF
* BRT 7 state quaternion EKF
* Watson 7 state quaternion EKF
* BRT cascaded Euler EKF
* Madgwick
* Mahony
* DCM
* Joop Brooking CF

I haven't gone into the math for each yet to see if some of these should be left out, there’s probably a lot of overlap between some of them, just wanted to get a complete list of algorithms to start.

Also, improvements to the MPU-9250 library:
* Configuration issues after calibration
* DMP (planned addition, not sure when I’ll get there)

Some improvements to the MPU-9250 library; although, these are not MPU-9250 specific and may be applied to any IMU:
* Improving / simplifying calibration routines
* No motion detection
* Magnetic interference detection
* Temperature compensation
 
For the AHRS library, these are the algorithms and improvements that I'm seeing, please let me know if I'm missing any:
Think you may want to consider that there are 2 implementations of Madgwick you may want to consider (1 is the original paper and the 2nd is the one that is posted). Other than that I think that's about it.

DMP (planned addition, not sure when I’ll get there)
From what I remember DMP on the 9250 is only the accel and gyro. Solution doesn't include magnetometer.

Some improvements to the MPU-9250 library; although, these are not MPU-9250 specific and may be applied to any IMU
Think that's it for improvements.

On a separate issue I just tried the uNav INS software and it keeps giving me Nans after a few iterations. Checked that I am getting raw data from the IMU and GPS and that all looks good. So not sure what the problem is. I did run the calibration and running at SRD 9.
 
Ok. I found the issue with the nan's. Back to an old problem with _dt. At about line 112 of the .cpp
Code:
_dt = (float) _t
needs to be changed to
Code:
_dt = (float)_t /1000000.0;
Then it works fine

EDIT: Almost forgot. In the example sketch you need to add back the loading of the calibration values.
 
Last edited:
Not sure which code is making the _t … The _t calc still puzzles me as to the perfect way. Using a fixed value for interval the MPU should produce the data samples at - or based on the observed/calculated interval between pulling the sample?

Tending to think the former is right by 'assumption' - the latter has two ways to get it either capture the _isr time hit, or when the calc 'happens' to be done. Originally it was based on the latter 'happens', but not sure the added way to record time at _isr is in current code which could be better if the MPU time interval slips over time.

Is using CAN to move the raw data to a second Teensy faster or better than using SPI_MST? Maybe the units are not side by side and CAN does better distance.
 
Right now was just trying out the new code that Brian referenced to see how it worked out. Ran into the "nan" issue right away and took a while to remember what we did in the past. Finally decided to check the _dt and sure enough it was the same issue that we had a lot earlier in thread. After that change and adding the calibration code back in then it ran. Haven't really tested the yaw stability yet.

Tending to think the former is right by 'assumption'
Believe it or not that is the way all the AHRS algorithms do the dt. So would probably stick with that for now.

Is using CAN to move the raw data to a second Teensy faster or better than using SPI_MST?
Don't know if better but a lot less wires. Essentially with CAN you are dealing with just 3wires (CAN-H, CAN-L and common Gnd), and distance is not a problem between Teensies. Haven't really tried it in an AHRS environment. Just wanted to see if it could be done:)

BTW. In just a preliminary test Yaw is still going diverging but I have an idea that I need to test out to confirm.
 
SPI_MST IS faster than CAN. However, CAN can go over long distances at most 1Mbps (unless Teensy 4 introduces CANFD, which is a different beast)

you can run up to 40 meters of UTP at 1Mbps
you can run up to 1000 meters of UTP at 50Kbps
they are rated for about 30 nodes
 
Think you may want to consider that there are 2 implementations of Madgwick you may want to consider (1 is the original paper and the 2nd is the one that is posted). Other than that I think that's about it.

From what I remember DMP on the 9250 is only the accel and gyro. Solution doesn't include magnetometer.

Think that's it for improvements.

On a separate issue I just tried the uNav INS software and it keeps giving me Nans after a few iterations. Checked that I am getting raw data from the IMU and GPS and that all looks good. So not sure what the problem is. I did run the calibration and running at SRD 9.

Thanks, I should have diff'd the two INS files (from the flight software repo and from the library). Ooops, thanks for the bug find.
 
Not a problem Brian.

Did some testing and when I started testing I still saw the yaw drifting again, not as bad as before but I was running with the GPS at 200hz and Pedestrian nav mode. But anyway I think I found the source of the yaw drifting as bad as it did but I think you all need to confirm it. Here is what I did:

1. Fixed lat, long for my current location and fixed velN, velE and velD to 0. Guess what no yaw drift.
2. Left velocities at 0 and used GPS data for lat and long. Still drifting but not as bad as before.
3. Changed Nav mode to portable and left update rate at 200hz and let it running for about 20minutes with no perceptible drift.

Think the problem that we were all struggling with is really caused by GPS data sitting on the bench with the antenna hanging by the window. So, at least for me I think I am going to keep those settings for test purposes. Also, maybe put a velocity test in, that if is less than a certain amount, that velocities are just zeroed out. Would have to print them out to really see what it should be.

If you use RTK or under open skies this issue may be resolved or at least minimized.

Going back to the list of improvements think I would do them in the following order:
1. Magnetic interference detection
2. No motion detection
3. Improving / simplifying calibration routines
4. Temperature compensation


Cheers
mike

EDIT: Forgot to add one thing. Pitch always shows up as -25 degrees for some reason as opposed to zero when IMU is flat. Not sure whats causing this problem.
 
Last edited:
Did a little more playing around with the constants and got semi-stable yaw drift by using the following set of constants:
Code:
    // Std dev of correlated gyro bias
    const float SIG_G_D = 0.00873f;   // 0.1 deg/s 0.00873f
    // Correlation time or time constant
    const float TAU_G = 25.0f;  //was 50
    // GPS measurement noise std dev (m)
    const float SIG_GPS_P_NE = 13.0f;  //original 3
    const float SIG_GPS_P_D = 15.0f;   //original 5
    // GPS measurement noise std dev (m/s)
    const float SIG_GPS_V = 5.5f;      //originally 0.5f

This is for my set up sitting at my desk with the antenna by the window. Don't quite understand the time constant for the gyro. If anybody has any other suggestions let me know. Oh, BTW, this version is better than previous versions.
 
For what it's worth, when we fly, we walk around with the aircraft for several minutes getting heading, pitch, and roll changes to warm up the filter with GPS input. I wouldn't expect great results without that warmup period. The heading is completely from GPS data (no mags) and all you're measuring when stationary is the GPS noise.
 
For what it's worth, when we fly, we walk around with the aircraft for several minutes getting heading, pitch, and roll changes to warm up the filter with GPS input. I wouldn't expect great results without that warmup period. The heading is completely from GPS data (no mags) and all you're measuring when stationary is the GPS noise.
That now makes perfect sense and explains the results I am seeing. It also explains why what I changed worked or did not work :).

Forgot to add and ask that this version of the uNav code is a bit different that the previous versions we were working with.
 
Last edited:
Brian,

I am having an issue with the calibrateMag method of the MPU9250 library. The calibrateAccel methofd seems to give me reasonable bias and scale float values,
but when I call the calibrateMag method and then the getMagBiasX_uT, and getMagScaleFactorX methods I get nan. I retried this several times and all I get
is nan from the calibrateMag activity. I am using a Teensy 3.2 which then talks to a Windows program via USB. I downloaded the library 3-4 months ago - is there
an update that might fix this?
 
Brian,

I am having an issue with the calibrateMag method of the MPU9250 library. The calibrateAccel methofd seems to give me reasonable bias and scale float values,
but when I call the calibrateMag method and then the getMagBiasX_uT, and getMagScaleFactorX methods I get nan. I retried this several times and all I get
is nan from the calibrateMag activity. I am using a Teensy 3.2 which then talks to a Windows program via USB. I downloaded the library 3-4 months ago - is there
an update that might fix this?

Getting nan's is weird. Can you attach your code and let me know which breakout board you're using?
 
Status
Not open for further replies.
Back
Top