MPU-9250 Teensy Library

I was partly attracted to the teensy 3.5/6 by their raw power and the FPU, because I've got a strong hunch I'm heading towards fairly serious data manipulation and fusion algorithms from all the various sensors - simply to get the best possible / usable accuracy for attitude, direction and altitude.

Yes, I was drawn to the Teensy 3.6 for the floating point unit. I tried running an extended kalman filter on Teensy 3.2 and could only get about a 20 Hz update rate, I'm expecting this to be much faster when I get around to implementing it on Teensy 3.6. I'm hoping it's at least in the hundreds of Hz, which would be a fast enough speed to move a lot of this sensor processing off SOCs like the BeagleBone Black or Raspberry Pi and onto the Teensy 3.6.

I was attracted to SPI after reading the Perfomance section at end of your MPU9250 page on Github. I'm wondering if this may create a conflict with SPI for the microSD card on T3.5/6 ? But even if it is, the Kickstarter lists "3 SPI ports (one with FIFO)" so presumably there is a way to use the other SPI ports (or will be :) ).

Off to play a bit more ....

Have fun! Should work fine; in theory as long as the chip select pins are different, multiple devices can be on the same SPI bus. Please let me know if it works or not, I haven't had an opportunity to test with multiple SPI devices yet.

Brian
 
Pro-tip, dude:

Old-school:

if(buff[0] == data) {
return true;
}
else{
return false;
}


Like a boss:

return buff[0] == data;
 
I tried running an extended kalman filter on Teensy 3.2 and could only get about a 20 Hz update rate

Without seeing the code, I'm going to make a blind guess it uses "double" (64 bit) instead of much faster "float" (32 bit). Functions like sqrt() or cos() are double. The float versions add "f", so they're sqrtf(), cosf(), etc.

If everything is 32 bit float, give Teensy 3.5 or 3.6 a try. They have hardware floating point for 32 bit floats, so it ought to run really fast.

Of course, that's all a guess without seeing any code. Something else (perhaps simple) could be wrong, limiting the overall speed....
 
Brian (or other gurus ... ),
I'm working up a test jig with two (Sparkfun) MPU9250's working side by side and I have a question you may be able to help with. As you would know the default I2C slave address for the MPU is 0x68 but it can be changed to 0x69 by pulling AD0 pin high. To quote the Invense datasheet "This allows two MPU-9250s to be connected to the same I2C bus." and I have got this working OK ... well sort of, see below.

But what about the Magnetometer? It is accessed in "pass-through" mode with an I2C slave address of 0x0C always, apparently with no means to change it, AFAIK. This seems to defeat the goal of having 2 MPU's on one bus, with no way to target each Magnetometer individually?

Am I undertsanding this correctly? TIA
 
Last edited:
Brian (or other gurus ... ),
I'm working up a test jig with two (Sparkfun) MPU9250's working side by side and I have a question you may be able to help with. As you would know the default I2C slave address for the MPU is 0x68 but it can be changed to 0x69 by pulling AD0 pin high. To quote the Invense datasheet "This allows two MPU-9250s to be connected to the same I2C bus." and I have got this working OK ... well sort of, see below.

But what about the Magnetometer? It is accessed in "pass-through" mode with an I2C slave address of 0x0C always, apparently with no means to change it, AFAIK. This seems to defeat the goal of having 2 MPU's on one bus, with no way to target each Magnetometer individually?

Am I undertsanding this correctly? TIA

Yes, the address is 0x68 with AD0 pulled low and 0x69 with AD0 pulled high. The magnetometer is on the MPU-9250 auxiliary I2C interface.
Screenshot from 2016-11-02 09:38:56.png

The way I interact with the magnetometer is by:
1. setting up the MPU-9250 as the master on its I2C aux interface (Master I2C Serial Interface in the block diagram)
2. setting MPU-9250 registers which tell the MPU-9250 the address, subaddress, and data to send over its aux interface
3. command the MPU-9250 to write (or read) all of that data

In this sense, the magnetometers are on their own I2C bus with the MPU-9250 as their master. Then the MPU-9250 is on your I2C bus with the Teensy as the master. In this way, you can have two MPU-9250's on the same bus without conflicting, including without magnetometers conflicting.

There is the option to put the MPU-9250 into bypass mode and interact directly with the magnetometers via the Serial Interface Bypass Mux in the block diagram. Then the magnetometer would be on your I2C bus with Teensy as the master. And, as you noted, you couldn't have both magnetometers on the same bus. So you wouldn't want to put both MPU-9250's into bypass mode at the same time. My code never puts the MPU-9250 into bypass mode, so you wouldn't have to worry about it. Mostly, this was done to make it easy to have both I2C and SPI communication with the MPU-9250, but there are other benefits as seen here.

Brian
 
Brian

Thanks for that clarification, in nice clear language ... helps me heaps.

As mentioned earlier I've got my MPU's 'generally' working with your library and Kris Winer's, but my latest efforts are based on Kris Winer's example sketches for MPU-9250 which brings all the code into the sketch rather than including a library. I've bastardised/refined his MPU9250BasicAHRS_t3.ino sketch quite a bit and ostensibly it is working fine on my T3.5 with 1 MPU, whether at address 0x68 or 0x69.

I was keen to update it to work on two MPU's on same I2C at once, but he uses bypass mode to access the AK8963 and your explanation clears up why this approach wasn't going to work anyway!

I'm keen to get a really solid understanding of the MPU9250's workings, especially what accuracy can reasonably be achieved, and what smart algorithms might be needed to extract actual current vehicle Y/P/R from noise and other signals (e.g vehicle runs on surfaces that aren't smooth). Hence my test-jig concept: to simultaneously exercise two identically oriented MPU's, mounted side by side.

So here's the crunch: I reckon I can run one MPU on Winer's code (on I2C) and the other simultaneously using your library (on SPI or another I2C bus). I see this as a good way to convince myself I've actually got the settings/calibrations and the software correctly sorted. This is going to take me some time, partly because Winer's approach seems quite different to yours ... although I could be wrong about that as I haven't looked deep into your library code yet. For example Winer has a lot of code for things like self tests, reading factory calibrations figures and user provided calibration figures to determine various bias values unique to that particular MPU chip. I presume your library handles these issues as well?

Ultimately I want to use the best of both approaches in my final solution. For example I prefer your use of hardware interrupts to manage incoming data.

Maybe I'm on a 'wild goose chase' or re-inventing the wheel ? I'd appreciate your thoughts ...
 
Last edited:
I got a few of the onehorse/Winer modules and worked with them minimally before I got distracted. The All-in-one-file approach is not pretty - but has a lot of run time and fine tuning/testing done. A start might be to rename the ino to .h and pull the setup() and loop() code out - and then move them to the actual INO sketch - and make prototypes for any function called from the INO - not a library yet - but at least out of the way.

I assumed there would be an easy way to take the delaying POLL calls out and move that code to an A/B/C_isr() ( making it interrupt safe ) and having it set a volatile flag A/B/C. Any where that code was called would be replaced by a if ( A_isr_flag ) {continue) else skip. For any code A or B or C that previously waited for the polling code to complete? As long as running with a partial update is okay they can be individually skipped if not flagged as ready - any that need to be read together would need to use that condition. First step would be to isolate/create the _isr() code and call it as it does now to assure you don't have missing data values or functionality. Then set the unit up to trigger the interrupts and rely on the flags. If you don't cycle the loop() completely as fast as samples come in then they could be buffered or discarded as appropriate. Reading Brian's code for that might show you the needed precedence or care needed.

Kris or Brian might have a better picture - but that is what I see . . . having not actually looked at it for about a year.
 
I got a few of the onehorse/Winer modules and worked with them minimally before I got distracted. The All-in-one-file approach is not pretty - but has a lot of run time and fine tuning/testing done. A start might be to rename the ino to .h and pull the setup() and loop() code out - and then move them to the actual INO sketch - and make prototypes for any function called from the INO - not a library yet - but at least out of the way.

To make things a little more interesting I've attached ZIP of my W-I-P sketch teensyMPU03_KW, While it's based on Winer's t3 example (using no external MPU library) you'll see I've done a *lot* of restucturing / refomatting including much of what you suggest @defragster. So it's now spread over 11 .ino and a couple of .h files. I am focussed on AHRS mode only, so I dropped the non-AHRS sections altogether. There's much improved (prettier) reporting to Serial and a Nokia 5110 LCD at same time (the LCD's not essential).

This generally works with one MPU, but dual MPU not working properly, for reasons discussed above. I say 'generally' works because although I was careful not to interfere with any of Winer's code and quaternions/maths, I have yet to verify actual data reported is all good after my broad fiddling / rearranging the whole sketch. (That's where my dual test jig comes in)

Thanks for your suggestions re: interrupt handling. I'll have to study it first (so I understand !) and think about how it would work with the interrupt methods Brian uses in his library examples. Stand by ... but don't hold yr breath :)

PS: the interrupt mechanism will also have to keep track of delta-t that is used in the quaternions and filter update rate calcs. This is just micros() since last reading so should be easy enuf. It's done in loop() at present.

View attachment teensyMPU03_KW.i.zip
 
Last edited:
Brian

Thanks for that clarification, in nice clear language ... helps me heaps.

As mentioned earlier I've got my MPU's 'generally' working with your library and Kris Winer's, but my latest efforts are based on Kris Winer's example sketches for MPU-9250 which brings all the code into the sketch rather than including a library. I've bastardised/refined his MPU9250BasicAHRS_t3.ino sketch quite a bit and ostensibly it is working fine on my T3.5 with 1 MPU, whether at address 0x68 or 0x69.

I was keen to update it to work on two MPU's on same I2C at once, but he uses bypass mode to access the AK8963 and your explanation clears up why this approach wasn't going to work anyway!

I'm keen to get a really solid understanding of the MPU9250's workings, especially what accuracy can reasonably be achieved, and what smart algorithms might be needed to extract actual current vehicle Y/P/R from noise and other signals (e.g vehicle runs on surfaces that aren't smooth). Hence my test-jig concept: to simultaneously exercise two identically oriented MPU's, mounted side by side.

So here's the crunch: I reckon I can run one MPU on Winer's code (on I2C) and the other simultaneously using your library (on SPI or another I2C bus). I see this as a good way to convince myself I've actually got the settings/calibrations and the software correctly sorted. This is going to take me some time, partly because Winer's approach seems quite different to yours ... although I could be wrong about that as I haven't looked deep into your library code yet. For example Winer has a lot of code for things like self tests, reading factory calibrations figures and user provided calibration figures to determine various bias values unique to that particular MPU chip. I presume your library handles these issues as well?

Ultimately I want to use the best of both approaches in my final solution. For example I prefer your use of hardware interrupts to manage incoming data.

Maybe I'm on a 'wild goose chase' or re-inventing the wheel ? I'd appreciate your thoughts ...

Yes, our use case is similar, only we're using these for airborne vehicles rather than surface vehicles.

My assumption is that a lot of the bias and scale factor estimation and correction would occur on the Teensy, so I haven't implemented functionality to write to the gyro or accelerometer offset registers. Basically, these registers allow you to remove biases in these 6 measurements. Kris' calibration code gathers 40 samples from all 6 channels, averages them and pushes these biases to the gyro and accelerometer offset registers.

The way I use interrupts is probably how you would like to do it for a single sensor. I'm using the built in DLPF to reduce the bandwidth and allow me to sample at a lower rate without worrying about aliasing. Then I have the MPU-9250 generate an interrupt on data ready at a specified output data rate. When the interrupt is received by the Teensy, I poll the sensor to grab the newest measurements. This is setup by the setFilt function in my library.

For multiple MPU-9250's, the approach may be slightly different. If small time skews between sensors are okay, then you would just treat them like the single sensor using interrupts to poll the sensor data. If you need the multiple MPU-9250's time sync'd very precisely, I would take a slightly different approach. Instead of using the DLPF and reducing the output data rate, I would setup the FIFO buffer on both MPU-9250 sensors and have the sensors write to the FIFO buffer at full rate. I would use interrupts to let me know when the FIFO should be read. Finally, I would use the FSYNC pin, either off an interrupt generated by the Teensy or have one MPU-9250 generate the interrupt and have the other MPU-9250 take that interrupt as an input to its FSYNC pin. The FSYNC take the lowest bit of the selected channel and uses that to signal where in the data stream the interrupt occurred. This allows you to then very precisely synchronize the measurements from the two MPU-9250's.

I don't have the FIFO or FSYNC functionality in my code, but it might be worth adding if there's a need for it.

Brian
 
plenty of useful ideas arising from your reponse, tks. I really need to go and study the datasheet a bit more before deciding the best approach.

meanwhile a question: As I understand (before re-reading datasheet!) the concept behind the DLPF is simply that actual changes in vehicle orientation & location (airborne or ground based) will change relatively slowly compared to noise / bumps in road / turbulence / etc and so the DLPF improves accuracy of reported orientation by eliminating / attenuating those higher frequency influences.

If I've got that right - what about effects of vehicle acceleration / braking / banking, does the DMP offer any smarts to cancel those out as well? Again my purpose for the MPU is only to establish actual yaw/pitch/roll at any point in time, irrespective of other forces. I will get altitude separately from the GPS and a BMP-180 air pressure sensor ( although I think this can be connected to the auxilliary I2C on MPU9250 so the DMP can use these inputs directly?)
 
Last edited:
I was re-reading Kris Winer's excellent article "Affordable 9-DoF sensor fusion" and found an answer to one of my questions above.
To quote "For the individual maker, as I will show, the use of the DMP is neither desirable nor necessary to get all the same functionality in a completely transparent fashion. I will discuss how to do this with open-source sensor fusion algorithms shortly." And his article goes on to explain why ...

So use of the DMP is not necessary, at least for my purposes.
 
Last edited:
plenty of useful ideas arising from your reponse, tks. I really need to go and study the datasheet a bit more before deciding the best approach.

meanwhile a question: As I understand (before re-reading datasheet!) the concept behind the DLPF is simply that actual changes in vehicle orientation & location (airborne or ground based) will change relatively slowly compared to noise / bumps in road / turbulence / etc and so the DLPF improves accuracy of reported orientation by eliminating / attenuating those higher frequency influences.

If I've got that right - what about effects of vehicle acceleration / braking / banking, does the DMP offer any smarts to cancel those out as well? Again my purpose for the MPU is only to establish actual yaw/pitch/roll at any point in time, irrespective of other forces. I will get altitude separately from the GPS and a BMP-180 air pressure sensor ( although I think this can be connected to the auxilliary I2C on MPU9250 so the DMP can use these inputs directly?)

Yes, the idea behind the DLPF is that you want to attenuate the high frequency noise without attenuating the vehicle dynamics, which typically occur at lower frequencies. Additionally, as a general rule of thumb, you need to sample at a frequency of at least twice the frequency of any expected sensor input to eliminate the possibility of aliasing. You could implement this filter yourself on the Teensy by sampling the MPU-9250 at full-rate and then low pass filtering; however, the full-rate for the gyros is 8000 Hz. One way to reduce the Teensy processing required is to use the MPU-9250 filtering and then sample the output at a lower rate.

In practice, you generally would choose a filter bandwidth that is at least half of your expected sample rate. So, for example, that's why for the "Interrupt" examples in my library, I chose a DLPF bandwidth of 41 Hz for a sample rate of 100 Hz. Expected vehicle dynamics are probably in the 1-5 Hz frequency range, so I could filter with a much lower bandwidth to reduce noise even further, but doing so I would also increase delay. For rigid vehicle control, that delay is typically not an issue, but it would have to be considered when assessing the stability margins for your control loop.

Unfortunately, things like vehicle acceleration and banking are directly in the same frequency range as the vehicle dynamics that you are trying to measure and control, so you can't filter them out. Aircraft in a long steady bank are actually a good way to "break" an AHRS and a reason why EKF's may offer better performance in high dynamic environments.

Brian
 
Yes, I was drawn to the Teensy 3.6 for the floating point unit. I tried running an extended kalman filter on Teensy 3.2 and could only get about a 20 Hz update rate, I'm expecting this to be much faster when I get around to implementing it on Teensy 3.6. I'm hoping it's at least in the hundreds of Hz, which would be a fast enough speed to move a lot of this sensor processing off SOCs like the BeagleBone Black or Raspberry Pi and onto the Teensy 3.6.

In this post https://forum.pjrc.com/threads/34808-K66-Beta-Test?p=107194&viewfull=1#post107194
are some performance figures for Paul's kalman filter and Madgwick and Mahony. Single-precision float is used to take advantage of hardware floating point of T3.5/3.6 and STM32L4
 
To make this work on a Teensy 3.6 with SPI1 (Pins 0-MOSI, 1-MISO, 31-CS, 32-SCK) should I just change the SPI.* references in the MPU9250.cpp to SPI1.*? I've tried it out and get the IMU init unsuccessful message. I've got an ethernet device already on SP0.
 
No you just wire up to those pins and initialise the IMU object like this:

Code:
MPU9250 IMU(31);       // tell library pin 31 is the SPI Chip Select

Bonus: you don't even need to change any of your SPI code !
 
To make this work on a Teensy 3.6 with SPI1 (Pins 0-MOSI, 1-MISO, 31-CS, 32-SCK) should I just change the SPI.* references in the MPU9250.cpp to SPI1.*? I've tried it out and get the IMU init unsuccessful message. I've got an ethernet device already on SP0.

I would think that should work. I don't see anything it is doing overly special with SPI. Note: I would make sure that you have released version of Teensyduino. At one point I had the default SCK1 on pin 20 instead of on pin 32. As for what pin to use for CS, I don't think it maters as I believe the code is simply driving it like a normal IO pin.

Edit: Although I am not sure if all three SPI busses support 20mhz speed
 
Last edited:
No you just wire up to those pins and initialise the IMU object like this:

Code:
MPU9250 IMU(31);       // tell library pin 31 is the SPI Chip Select

Bonus: you don't even need to change any of your SPI code !

Actually, that only changes the CS pin for the SPI0, I was able to verify that SPI1 would cause the output of the Teensy to be assigned to the correct pins which required changing the .cpp file from SPI.* to SPI1.*

*However*, I'm using a Sparkfun board SEN-13762 and the 9250 AD0 pin is jumpered to ground to set the I2C address, this is also a shared pin with the 9250 MISO port. This is why I can see the correct activity on MOSI, SCK, CS but nothing on MISO. I remembered this from working with the board a month ago but my VM crashed and I lost all my code and notes from this project... I guess I learned the hard way, twice now.

This is the begin code for the SPI branch, it doesn't do any assignment based on CS pin.

if( _useSPI ){ // using SPI for communication

// setting CS pin to output
pinMode(_csPin,OUTPUT);

// setting CS pin high
digitalWriteFast(_csPin,HIGH);


// begin the SPI
SPI.begin();
 
Last edited:
Not so according to Brian's MPU-9250 library documentation nominating the CS pin is all important for SPI use, see the section headed "SPI Object Declaration" a few paragraphs down ...
Yes - you probably need to use an CS pin with the hardware, but it can be any digital pin on the Teensy.
If you look at the code: https://github.com/bolderflight/MPU9250/blob/master/MPU9250.cpp
Look at about line 720 and you will see it just uses digitalWriteFast commands to control the CS pin. It is not like the ILI9341 code which encodes which hardware CS pins into the actual instructions that are passed to the SPI subsystem.
 
Yes - you probably need to use an CS pin with the hardware, but it can be any digital pin on the Teensy.
If you look at the code: https://github.com/bolderflight/MPU9250/blob/master/MPU9250.cpp
Look at about line 720 and you will see it just uses digitalWriteFast commands to control the CS pin. It is not like the ILI9341 code which encodes which hardware CS pins into the actual instructions that are passed to the SPI subsystem.

Yes, exactly. The CS pin is treated just like a digital I/O, but the MOSI, MISO, and SCK are all still SPI0. Updating the library to make it easy to switch SPI buses, like how the I2C bus can be selected, is at the top of my short list.

EBRAddict, is it working for you now?

Thanks,
Brian
 
Back
Top