From 06f37aad75557854fbffbb779c96adb09f660bcc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 24 Feb 2012 11:32:20 +1100 Subject: [PATCH] DCM: use calculate_euler_angles() to get eulers from DCM this makes the code a bit easier to understand --- libraries/AP_DCM/AP_DCM.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index 7d1d04141e..443b23365e 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -483,9 +483,7 @@ AP_DCM::euler_angles(void) pitch = safe_asin((_accel_vector.x) / (double)9.81); // asin(acc_x) yaw = 0; #else - pitch = -safe_asin(_dcm_matrix.c.x); - roll = atan2(_dcm_matrix.c.y, _dcm_matrix.c.z); - yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x); + calculate_euler_angles(_dcm_matrix, &roll, &pitch, &yaw); #endif roll_sensor = degrees(roll) * 100; @@ -500,8 +498,7 @@ void AP_DCM::euler_rp(void) { check_matrix(); - pitch = -safe_asin(_dcm_matrix.c.x); - roll = atan2(_dcm_matrix.c.y, _dcm_matrix.c.z); + calculate_euler_angles(_dcm_matrix, &roll, &pitch, NULL); roll_sensor = roll * DEGX100; //degrees(roll) * 100; pitch_sensor = pitch * DEGX100; //degrees(pitch) * 100; } @@ -509,7 +506,7 @@ AP_DCM::euler_rp(void) void AP_DCM::euler_yaw(void) { - yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x); + calculate_euler_angles(_dcm_matrix, NULL, NULL, &yaw); yaw_sensor = yaw * DEGX100; //degrees(yaw) * 100; if (yaw_sensor < 0)