That code looks awesome. It's as if you wrote if for me. I don't understand much of the math but it doesn't matter as long as it works. I have one question and one concern.
Why are AHRS algorithms called filters? My engineering experience says a filter reduces things, like frequency in an audio filter or removing some variable. It seems to me that AHRS does PID stuff (minus the D) to update an equivalent number of vars.
My concern is how much effort will be involved to calibrate each 6050 in a production environment for a cheap toy. In yuor experience can the calibration be somewhat automated of at least quickly done manualy if not? I admit I haven't figured out how to do the calibration. I will need to do some studying. Maybe you could point me to something on the web about performing the calibration?
I really appreciate your code and your time helping me.