IMU 100hz Firmware

I finally got my Razor 9D0F IMU running at 100hz. The problem with the 8mhz atmega328p is that DCM calculations take more than 5ms. Knowing that 1 loop cycle can max be 10ms to get a 100hz output it's very hard to get all other things done withing this time. DCM code itself is to much math for me so I'm only able to tune output/input speeds.

The original Razor 9Dof AHRS firmware has a main loop running at 50hz. This main loop contains an subloop running at 10hz for compas readings. All i2c data is read at 100000bps and uart output is at 57600bps. So every 20ms this board pushes an update over the serial.

My own new AHRS firmware  has a main loop running at 100hz. So every 10ms my firmware pushes an update over the serial. To achieve the 100hz rate all i2c data comunication is at 400000bps and uart output is at 250000bps! I also tuned the serial output code. The original AHRS output code is simple arduino style, but it is really slow that way. Main loop runs at 100hz and my subloop runs at 50hz. My firmware has a faster update rate and should also be more accurate.