Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00
Code Issues Packages Projects Releases Wiki Activity

added ability to get the transpose of the DCM

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1116 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2010-12-13 04:01:26 +00:00
parent e3e8dbb0b3
commit 7945937874
2 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_DCM/AP_DCM.cpp
View File

@ -72,6 +72,13 @@ AP_DCM::get_dcm_matrix(void)
return _dcm_matrix; return _dcm_matrix;
} }
/**************************************************/
Matrix3f
AP_DCM::get_dcm_transposed(void)
{
return _dcm_matrix.transpose();
}
//For Debugging //For Debugging
/* /*
@ -284,7 +291,7 @@ AP_DCM::drift_correction(void)
cos_psi_err = cos(ToRad(_gps->ground_course/100.0) - yaw); cos_psi_err = cos(ToRad(_gps->ground_course/100.0) - yaw);
sin_psi_err = sin(ToRad(_gps->ground_course/100.0) - yaw); sin_psi_err = sin(ToRad(_gps->ground_course/100.0) - yaw);
// Rxx = cos psi err, Rxy = - sin psi err, Rxz = 0 // Rxx = cos psi err, Rxy = - sin psi err, Rxz = 0
// Ryx = sin psi err, R yy = cos psi err, Ryz = 0 // Ryx = sin psi err, Ryy = cos psi err, Ryz = 0
// Rzx = Rzy = 0, Rzz = 1 // Rzx = Rzy = 0, Rzz = 1
rot_mat.a.x = cos_psi_err; rot_mat.a.x = cos_psi_err;
rot_mat.a.y = - sin_psi_err; rot_mat.a.y = - sin_psi_err;

1
libraries/AP_DCM/AP_DCM.h
View File

@ -47,6 +47,7 @@ public:
Vector3f get_gyro(void); Vector3f get_gyro(void);
Vector3f get_accel(void); Vector3f get_accel(void);
Matrix3f get_dcm_matrix(void); Matrix3f get_dcm_matrix(void);
Matrix3f get_dcm_transposed(void);
void set_centripetal(bool b); void set_centripetal(bool b);
void set_compass(Compass *compass); void set_compass(Compass *compass);