This is how I convert from quaternions to radians:
Code: Select all
Eigen::VectorXf q(4); q << m_imu->qw, m_imu->qx, m_imu->qy, m_imu->qz; // normalize the quaternion q /= q.norm(); // calculate Yaw return atan2( 2.F * (q(1) * q(2) + q(3) * q(0)), 1.F - 2.F * (q(2) * q(2) + q(3) * q(3)));
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.