Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-12 10:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

DCM: use calculate_euler_angles() to get eulers from DCM

Browse Source 
this makes the code a bit easier to understand
This commit is contained in:
Andrew Tridgell 2012-02-24 11:32:20 +11:00
parent df6013616e
commit 5009679617
1 changed files with 3 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_DCM/AP_DCM.cpp
View File

@ -483,9 +483,7 @@ AP_DCM::euler_angles(void)
pitch = safe_asin((_accel_vector.x) / (double)9.81); // asin(acc_x) pitch = safe_asin((_accel_vector.x) / (double)9.81); // asin(acc_x)
yaw = 0; yaw = 0;
#else #else
pitch = -safe_asin(_dcm_matrix.c.x); calculate_euler_angles(_dcm_matrix, &roll, &pitch, &yaw);
roll = atan2(_dcm_matrix.c.y, _dcm_matrix.c.z);
yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x);
#endif #endif
roll_sensor = degrees(roll) * 100; roll_sensor = degrees(roll) * 100;
@ -500,8 +498,7 @@ void
AP_DCM::euler_rp(void) AP_DCM::euler_rp(void)
{ {
check_matrix(); check_matrix();
pitch = -safe_asin(_dcm_matrix.c.x); calculate_euler_angles(_dcm_matrix, &roll, &pitch, NULL);
roll = atan2(_dcm_matrix.c.y, _dcm_matrix.c.z);
roll_sensor = roll * DEGX100; //degrees(roll) * 100; roll_sensor = roll * DEGX100; //degrees(roll) * 100;
pitch_sensor = pitch * DEGX100; //degrees(pitch) * 100; pitch_sensor = pitch * DEGX100; //degrees(pitch) * 100;
} }
@ -509,7 +506,7 @@ AP_DCM::euler_rp(void)
void void
AP_DCM::euler_yaw(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; yaw_sensor = yaw * DEGX100; //degrees(yaw) * 100;
if (yaw_sensor < 0) if (yaw_sensor < 0)