Beispiel #1
0
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
}
Beispiel #2
0
		Fix16 asin() { return Fix16(fix16_asin(value)); }