Loaded your sketch with dependencies then calibrated with your complete and compiled python exe. I got a good sphere using the routine as suggested
https://github.com/mjs513/FreeIMU-Updates/wiki/04.-FreeIMU-Calibration.
Saved calibration.h to /Arduino/libraries/FreeIMU/
Arduino Rebuilt loaded and rebooted Teensy
WARNINGS (not errors)
psIMU_serial: In function 'float invSqrt(float)':
psIMU_serial:370: warning: dereferencing type-punned pointer will break strict-aliasing rules
uint32_t i = 0x5F1F1412 - (*(uint32_t*)&x >> 1);
^
psIMU_serial:371: warning: dereferencing type-punned pointer will break strict-aliasing rules
float tmp = *(float*)&i;
^
MadgwickQuantFileter: In function 'void MadgwickQuaternionUpdate(float, float, float, float, float, float, float, float, float)':
MadgwickQuantFileter:15: warning: variable '_2q0q2' set but not used
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
^
MadgwickQuantFileter:15: warning: variable '_2q2q3' set but not used
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
^
MahoneyQuantFilter: In function 'void MahonyQuaternionUpdate(float, float, float, float, float, float, float, float, float)':
MahoneyQuantFilter:48: warning: 'halfex' may be used uninitialized in this function
halfex += (my * halfwz - mz * halfwy);
^
MahoneyQuantFilter:49: warning: 'halfey' may be used uninitialized in this function
halfey += (mz * halfwx - mx * halfwz);
^
MahoneyQuantFilter:50: warning: 'halfez' may be used uninitialized in this function
halfez += (mx * halfwy - my * halfwx);
^
Press y in serial output to get Quaternion values for XYZ and all I get is
NANNANNAN
NANNANNAN
SOOOO?
C for calibration data reads normal (installed) values.
r 20 records reads values with no nan. Everything looks normal.
z for MultiWii data shows hex values. MultiWii interface shows all data EXCEPT for xyz quaternion, NAN.
How do I fix it so it spits out quaternion values for me (with your fix for the yaw drift?)
Thanks!