void Euler_angles(void)
{
// printf("%f,%f,%f\n",fix16_to_dbl(-DCM_Matrix[2][0]),fix16_to_dbl(DCM_Matrix[2][1]),fix16_to_dbl(DCM_Matrix[2][2]));
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
roll = fix16_atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z)
pitch = fix16_atan2((Accel_Vector[0]),Accel_Vector[2]); // asin(acc_x)
yaw = 0;
#else // Euler angles from DCM matrix
pitch = fix16_asin(-DCM_Matrix[2][0]);
roll = fix16_atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
yaw = fix16_atan2(-DCM_Matrix[1][0],-DCM_Matrix[0][0]);
#endif
}
Fix16 asin() { return Fix16(fix16_asin(value)); }