void quat_2_euler( float q[4], float e[3] ){
float sqw = q[0]*q[0];
float sqx = q[1]*q[1];
float sqy = q[2]*q[2];
float sqz = q[3]*q[3];
e[2] = atan2f(2.f * (q[1]*q[2] + q[3]*q[0]), sqx - sqy - sqz + sqw);
e[1] = asinf(-2.f * (q[1]*q[3] - q[2]*q[0]));
e[0] = atan2f(2.f * (q[2]*q[3] + q[1]*q[0]), -sqx - sqy + sqz + sqw);
}