I am using the BNO055 in order to visualize the orientation of a hand in 3d space.
My goal is to visualize the orientation in all the 3 axis (heading, roll, pitch), and for that I am using the Quaternion data provided by the sensor, in order to avoid the singularities associated with the Euler angles.
However, I am still encountering some problems: when the roll value reaches the 90 degrees, I notice an increase in the pitch value.
I assume that this problem can be solved with the addition of some code that handles specific cases that produce singularities, but until now I was not able to find a solution.
Please advise.
Here is the code I use (Wikipedia) to get the Quaternion data and convert them to Euler angles. (https://en.wikipedia.org/wiki/Conversio ... ler_angles)
I have tried to add the code from here (http://www.euclideanspace.com/maths/geo ... onToEuler/) in order to handle the singularities but it did not seem to work.
double x = QUA_DATA_X_BNO055();
double y = QUA_DATA_Y_BNO055();
double z = QUA_DATA_Z_BNO055();
double w = QUA_DATA_W_BNO055();
double d = sqrt((pow(x,2))+(pow(y,2))+(pow(z,2))+(pow(w,2)));
double yn = y / d;
double xn = x / d;
double zn = z / d;
double wn = w / d;
float temp = xn;
xn = - zn;
zn = temp;
yn = -yn;
double sqw = wn*wn;
double sqx = xn*xn;
double sqy = yn*yn;
double sqz = zn*zn;
//Source Wikipedia
// roll (x-axis rotation)
double sinr_cosp = 2.0 * (wn * xn + yn * zn);
double cosr_cosp = 1.0 - 2.0 * (xn * xn + yn * yn);
double roll = atan2(sinr_cosp, cosr_cosp);
roll = roll * 180.0/3.14;
//pitch (y-axis rotation)
double sinp = 2.0 * (wn * yn - zn * xn);
if (sinp >= 1.0)
pitch = 3.14 / 2.0; // use 90 degrees if out of range
else if(sinp <= -1.0)
pitch = -3.14 / 2.0;
else
pitch = asin(sinp);
pitch = pitch * 180.0/3.14;
// yaw (z-axis rotation)
double siny_cosp = 2.0 * (wn * zn + xn * yn);
double cosy_cosp = 1.0 - 2.0 * (yn * yn + zn * zn);
double heading = atan2(siny_cosp, cosy_cosp);
heading = heading * 180.0/3.14;