/* Calculates the Euler angles (roll, pitch, yaw) from the DCM matrix.  */
void IMU_dcmGetAngles(float dcm[3][3], float angle[3])
/* Calculates the Euler angles (roll, pitch, yaw) from the DCM matrix.  */
void IMU_dcmGetAngles(float dcm[3][3], float angle[3])
{
   angle[0] =  atan2f( dcm[2][1], dcm[2][2] );
   angle[1] = -asinf( dcm[2][0] );
   angle[2] =  atan2f( dcm[1][0], dcm[0][0] );
   return;
}