This is how I convert from quaternions to radians:
Code: Select all
q << m_imu->qw,
// normalize the quaternion
q /= q.norm();
// calculate Yaw
2.F * (q(1) * q(2) + q(3) * q(0)),
1.F - 2.F * (q(2) * q(2) + q(3) * q(3)));
I guess, even if I mistook something in the formula, it should not oscilate between [-3.14; 3.14] since it was not moving at all.
As settings, I have:
IMU Mode: High performance
Raw inertial sensor data: enabled
Processed IMU data: enabled
(IMU via modem)
Output mode: IMU fusion data
IMU fusion location: enabled
IMU fusion quaternion: enabled
Also, at some point, it froze. Like, it was always returning the same angle. After I restarted the beacon, started oscillating again. I can't guarantee it wasn't a connection problem to the serial port...
Anyway, if you have any idea, please let me know! Thanks.