Hi Brian. Hope you are still with us. I started working the quaternion version of uNavAHRS again, yes, I am a glutten for punishment. So I decided to start at top and work my way down. I am starting with the quaternions, and it has taken me months to realize that in addition to swapping x and y axes you negated the z axis so accel/gyro is aligned with the magnetometer axes. So where I am now is with a few confirmation type questions and other questions that I hope you don't mind my asking:
1. Your axes rotations seems to align them with what is contained in the Vector Nav App Note AN002 (Quaternion math)?
2. Am assuming you are keeping with the 321 (PRY, or yxz) Euler angle rotation as shown in the paper?
3. In looking at the initialization for _Quat it would seem to confirm item 1, but when I look at your get quaternion function it appears they return the q values in the wrong order
IS:
Code:
*qw = _Quat(0,0);
*qi = _Quat(1,0);
*qj = _Quat(2,0);
*qk = _Quat(3,0);
Should Be (I think):
Code:
*qi = _Quat(0,0);
*qj = _Quat(1,0);
*qk = _Quat(2,0);
*qw = _Quat(3,0);
4. If assumptions 2 is correct then should the initialization for the Euler angles be based on the equations 28 and 28 in Freescale App Note AN3461 (Tilt Sensing Using a Three-Axis Accelerometer) and equation 22 in AN4248 which looks like what you are doing although have to satisfy myself on the format you are doing. (Man, you are bringing me back to what I used to do in grad school).
5. Assuming my assumption in 2 and 3 are correct then I think your euler to quat function doesn't match whats in AN002. Not sure yet.
Ok that's about it for now until i get through the rest.
Thanks
Mike