uNav AHRS

Status
Not open for further replies.
Indications so far the T_3.6 can handle it from my speed test. Compile your current version at 48 Mhz and see if it needs to be bumped higher to run the same as your default 240 (180?) MHz builds. If it can run under half speed - then adding a second even more intense filter set should work. That was my goal in running at 24 and 48 to see how much headroom there was after pulling the samples and running the Euler version.

If I have some idea this might lead to a solution to a background task I had in mind - I could more be helpful with the SD and such mechanics than the math you three seem to have well in hand. And as Brian just added - if it would use a second Teensy for logging would be really fun :). I've watched the SD card usage and libs go by - SD latency does seem to bite at times. A second dedicated pass through logger (T_3.5 or T_3.6 ) could simplify and not impact the TestTeensy. A defined standard would make that easier for group use.
 
Ok I'm running behind here.
Don:
I'm going to spend a couple of hours this afternoon and resurrect my Mahony and Madgwick code (Winer's code originally) and get it working using Brian's MPU9250 library.
Click to expand...
It should work no problem. I ported a lot of Brian's library over to my FreeIMU library for use with the 9250 and works like a charm. I have multiple filters I can choose including DCM. Of course I do some other stuff with the sensor data first like run it through a filter. Of course for side by side comparisons you are going to need to have incorporate the filters into the code. I have the madgwick, Mahoney and DCM filters broken out into header files that you can just call. If you want I can post them then you can just call them.

defragster:
or the VUSB line could be cut to power from Bread board to allow USB to come and go with Teensy staying live. When it comes to GPS testing - that could end up being really useful to leave the desk and take a walk.
Click to expand...
Already done that when I was testing the gps outside and sending back the data to the pc for logging. Speaking on the discussion of logging, if you are streaming to Tviewer there is already an option to save the data as a csv file, if you want to go that route. Personnally I'd rather stay away from the SD card for just personal reasons.

brian:
For simple logging for debug purposes, my favored approach on a linux system is to use minicom instead of the arduino serial monitor and to stream the minicom data to a file.

For my flight software stack, I stream data to a BeagleBone Black which writes the data to its EMMC.
Click to expand...
All I can say is nice. Don't really work with Linux much but I do have a raspberry pi 3 laying around that I'm not using. So I have to keep this approach in mind.

defragster:
Indications so far the T_3.6 can handle it from my speed test.
Click to expand...
Don't doubt it.
 
Forgot. Update on the DCM-Kalman filter. Got it working by accident. Was trying to debug why I would always get Nan for YPR so I added a simple print statement at the beginning of the update filter to check the raw data. It stated working after that, if I removed same behavior. I had to add a 1 ms delay to get it to start working. Anyway, once I did that pitch and roll looked good but yaw varied between -180/+180 even if I rotated the board. If I just checked the pitch and yaw it would work for a while and then blow up. Anyone have any suggestions. I am going to continue working on it periodically.
 
Congrats Mike! Glad you got it working. Am still debugging my Mahony and Madgwick... if mine doesn't work tomorrow I may ask for yours! :)

Logging the CSV files (rather than using SD card) sounds like a great idea. I'll plan that route for now.

I have a couple of Pi boards too, so can go that way too if folks want...

Don
 
Mike, you mentioned "I have the madgwick, Mahoney and DCM filters broken out into header files that you can just call. If you want I can post them then you can just call them." I've been fighting most of the day with trying to get Mahoney and Madgwick to work with Brian's MPU9250 library. I like his library because everything is referenced to NED body. Do you have Mahony and/or Madgwick working with his library, and if so can you post? Thx!
 
Hi Don. I haven't tried it in a standalone format. I have most of the factors and one routine embedded in a major library that is rather general in nature where I can select pretty any imu out there. Pretty much all you do is pass the acceleration, mag and gyro to the filter and it returns the quat's. Then the my lib does the conversion. I can give it a shot as a standalone and then forward them to you. Shouldn't be too big of a deal. Have to do it later tonight - you are not in a hurry are you?
 
Thats the kind of thing i was starting to pull together: Complementary, Mahony (w and w/o mag), Madgwick (w and w/o mag), EKF Euler, EFK Quaternion... I had Winer's Mahony and Madgwick working using a different MPU9250 library and thought it would be easy to just switch to Brian's. I even went back and ported in Madgwick's original C code, but I'm having issues with roll blowing up so have something (probably simple) messed up.
 
Don Kelly said:
Thats the kind of thing i was starting to pull together: Complementary, Mahony (w and w/o mag), Madgwick (w and w/o mag), EKF Euler, EFK Quaternion... I had Winer's Mahony and Madgwick working using a different MPU9250 library and thought it would be easy to just switch to Brian's. I even went back and ported in Madgwick's original C code, but I'm having issues with roll blowing up so have something (probably simple) messed up.
Click to expand...

We could put together a library of different AHRS classes and use a common set of methods for all of them so that it's easy to switch between.
 
Brian - Don

I will go ahead and put together the Madgwick, Mahoney and DCM filter. I actually have two versions of Madgwick, yes there are two, one was in his original paper and the second is what was posted - very similar but different. I have it set up currently that I can use the versions with or with the magnetometer. Unfortunately, you have to use the Mag with the DCM. Then you all can build on it if you want.

Mike
 
Mike,
That'd be fantastic! Tomorrow I'll work on the simple Complementary Filter I used on my first drone, it's from Joop Brokking, works very well, no mag inputs.

I've had frustrating day with Madgwick and Mahony. I had it working with Winer's chipset, but he used the default orientation for the MPU, which is not NED body (and mag is different from accel and gyro). But it seemed that the Madgwick and Mahony were NED, but I've tried every combo of signs on the inputs and haven't got it right. Ggggrrrr... So I still have some sort fo error in my filter calc or my quat2Euler routine (which I have two versions and have checked both).
 
Hi Don

Know what you mean. I asked a similar question in an earlier thread about getting axes aligned with the 9250. Anyway have the 3 filters incorporated into a sketch now just working the bugs then for the serious testing. Had fun trying to incorporate into a single sketch.
 
Don - Brian

Attached is a working copy of all the filters I mentioned in my previous post. Consider this a rough cut - have some more work to do but it does work.
 

Attachments

  • MPU9250_Filters.zip
    16.7 KB · Views: 114
Awesome!! I just pulled in Joop's complementary filter and am getting it ported over to work with Brian's MPU9250 library. Joop's version was designed for reading the raw MPU6050 registers and then converting those register values (integers) to accel and gyro readings, so I have a bit of converting and testing to do.

What I like about the complementary filter is that it's so simple and eloquent, and it works (at least for me using the 6050) pretty darn well at 250 Hz. It really is just a pitch/roll estimator, but does provide rough yaw number.
 
Last edited:
Here's the sketch with the complimentary filter... all working with Brian's MPU9250 library.

Note that this complementary filter just uses gyro data to estimate pitch and roll, and accel data to offset gyro drift. I provide a yaw estimate, but it's just a simple estimate using the gyro, so has significant drift.

Don

View attachment CompFilterV2.zip
 
Mike, thx for the paper. The author shows an algorithm for yaw from magnetometers in a nice form, so I may add that to my complementary filter routine so that I'll have both PR and PRY versions, sort of like I've seen with the Madgwick and Mahony filters.

For my early drone work, heading wasn't that important. I was trying various PID auto-level schemes and motor commanding control. But now, with my forage into RTK and auto-routing, heading is really important!
 
Mike,
I'm getting sign errors in the outputs of the four MARG filters in your latest sketch, much like I was getting yesterday with the Mahony and Madgwicks I was running. For example, for MARG 1 and 0 I'm getting

- Yaw sign reversed
- Pitch sign correct (but value too low for MARG 0)
- Roll sign reversed

Are you assuming a body NED frame, so
- rotation about Y-body axis gives positive pitch (nose up)
- rotation about X-body axis gives positive roll (left wing up)
- rotation about Z-body axis gives positive yaw (nose to right)

I think Brian's MPU9250 library aligns with the X-Y-Z (gyro, accel) markings on my SparkFun chip, and it seems to be working ok with my complementary filter code. I'm thinking we might need to put the inputs into the MARG filters in a different order to match NED body assumptions. For example, a pos gyro rotation about Y really increases pos pitch and pos gyro rotation about x really increases pos roll. So am wondering if we need to input gyro values as Y, X, Z. What do you think? If this isn't the issue, then it might be that the MARG filters are assuming the inputs are in ENU. Aaaarrrggg, confusing...
 
I just tried

MadgwickAHRSupdateIMU(gy,gx,-gz,ay,ax,-az)

...and it gave me a NED solution for PRY but rotated in the horizontal by 90 deg. So it's almost like the Madgwick filter wants the inputs in ENU. enough for tonight, no other ideas right now...
 
One of the things I want to say is that the filters want the z components lined up and pointed up. The code I posted is mz = -Imu.getMagZ_rads. I am still not sure the axes are correct. I am going to work on it tonight. Am having one problem your compfilter - it prints out once then hangs - know I am missing something but not sure what.
 
Last edited:
Nope. I was experiencing the same thing after you post and was driving me crazy. I changed the gravity direction in the getYawPitchRoll function in the global tab to:
Code: 
void getYawPitchRollRad(float * ypr) {
  float grx, gry, grz; // estimated gravity direction
  
  //grx = 2 * (q[1]*q[3] - q[0]*q[2]);
  //gry = 2 * (q[0]*q[1] + q[2]*q[3]);
  //grz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
  //calculate direction of gravity
  grx = 2 * (-q[1]*q[3] + q[0]*q[2]);
  gry = 2 * (-q[0]*q[1] - q[2]*q[3]);
  grz = -q[0]*q[0] + q[1]*q[1] + q[2]*q[2] - q[3]*q[3];

  ypr[0] = atan2f(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1);
  ypr[1] = -atanf(grx / sqrt(gry*gry + grz*grz));
  ypr[2] = atanf(gry / sqrt(grx*grx + grz*grz));
}
and it seems to working much better. Give it a try if you get a chance. By the way there is one typo. For the disabled magnetometer case you need to add IMU after the update so AHRSupdate(gx1, gy1, gz1, ax1, ay1, az1); should be AHRSupdateIMU(gx1, gy1, gz1, ax1, ay1, az1);

Cheers
Mike

EDIT: Oh by the way I did do a recal and it helped.
EDIT1: just remember the nose is pointed in the y-direction marked on the breakout board.
EDIT2: Forgot to mention there is a calcMagHeading function in the global's tab that can be used with your complimentary filter.
 
Last edited:
Don - I updated the Filters sketch with your Complimentary filter and the corrections I mentioned in my previous post. I am attaching the updated sketch. Still have to test all the filters but now the fun starts.

Mike
 

Attachments

  • MPU9250_Filters.zip
    18.3 KB · Views: 147
Don Kelly said:
... , you're using INT on pin 14, I'm using INT on pin 1. That might be it...
Click to expand...

This is why I was posting hacks to the sketches I touched with the #define - with a common set of those interchangeability would be easier - especially for buried things like SPI, alternate i2c wires and changed Serial#'s for GPS/Debug. And isr() setup on INT pins.
That will also relate to catching the GPS_Serial# Rx isr() pin.

#define IMU_ADDR 0x69 // 0x68 // SPI 9
#define IMU_SCL 47 // 0x255
#define IMU_SDA 48 // 0x255
#define IMU_INT_PIN 50 // 1 // 14
#define coutD_BAUD 2000000 // coutD for DEBUG port
#define coutD Serial1 // Serial2
#define GPS_Start 2 // 1 // UBLOX gps(gps_Start);
#define GPS_PORT Serial2 // Serial1
#define GPS_BAUD 460800
#define GPS_AltRx 255 // 27
#define IMU_BUS Wire // SPI
#define IMU_SPD 1000000 // 0==NULL or other
Click to expand...
 
degragster

Can't argue with the logic. Thought about it but got a little lazy. Anyway, I updated the sketch with your changes and while I was at it I incorporated the uNavEul filter by Brian. The sketch needs some commenting on where things came from but it is functional now I think ...

Thanks
Mike
 

Attachments

  • MPU9250_Filters.zip
    24.2 KB · Views: 126
Mike,
On my SparkFun MPU92050 board, there's a NED axis labeled for the accel and gyro axes. For a NED body frame, usually what's assumed is
- X points through nose of aircraft
- Y points through right wing
- Z points down through body of aircraft

so that means, using right-hand-rule
- pos rotation about x lifts the left wing (pos roll)
- pos rotation about y lifts the nose (pos pitch)
- pos rotation about z moves nose to right (pos yaw)

I think the trick is figuring out what assumptions Mahony and Madgwick need for us to be in sync with Brian's library. Also, I've been assuming they need accel with gravity left in, but it may be that they need gravity components removed first...

I've put together a cool little sketch that uses Brian's NED MPU9250 and helps to verify that XYZ is what we think it is on the board. I'll clean it up and post with instructions. I'm thinking this will help us make sure we all have our axes aligned as I'm thinking Brian's library assumes.
 
Status
Not open for further replies.
Back
Top