New Madgwick Fusion Filter

@brtaylor and all interested.

I couldn't resist thetrying to see if I could get the ICM20948 working. So I updated your library with a version for the ICM20948:

Consider it still a work in progress as I haven't included Wake on Motion functionality yet but most is there including a couple new functions that are necessary. I included an example using the Fusion library and I2C interupt version. If you want give it a try and let me know if its worth including as a PR.

Sweet! No need for WOM. My goal is to just have basic functionality working across the line of devices. I'll try it out.
pushed a couple of minor changes and documented the commands in i2c_interrupt example
A lot of these filters are only valid for static, or for very low speed application .
As soon as we use them on a bike, motorbike, car, plane, etc..., they suffer from centripetal forces.
Correction could be done with either a GPS module, an encoder or a serial value received.
Is there any plan to add this correction ???

Last edited:
But if it does work and if possibility, then next would be to see about moving the variant completely out of the Teensy install. Could be own install or in <sketches>/hardware... But not sure how fully yet.
Just tested it and yep it interrupt version works with ICM-20649.
@mjs513, thanks! I created a branch of my library here to work on integrating the ICM-20649:

It seems to work with STM32, but I can't communicate with the sensor using SPI and Teensy 3.2. Working on trying to figure that out now.
Seems to work on SPI Mode 0 instead of Mode 3.

Noticed a note about setting I2C_IF_DIS immediately on power up, going to see what that does. Still need to test the DRDY.
Looks like setting I2C_IF_DIS right after boot enables SPI Mode 3 to work with Teensy.
Glad you found that - don't think I tested SPI as thouroughly as I2C.

Sorry took so long was recovering from snow removal :) not much but enough