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

Added dynamic setting of compass

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1010 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2010-12-04 06:24:21 +00:00
parent 126383fe43
commit 2f2aaa88fd
3 changed files with 12 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_DCM/AP_DCM.cpp
View File

@ -31,8 +31,11 @@
#define ADC_CONSTRAINT 900
// Constructors ////////////////////////////////////////////////////////////////
void
AP_DCM::set_compass(Compass *compass)
{
_compass = compass;
}
/**************************************************/
void
@ -263,8 +266,10 @@ AP_DCM::drift_correction(void)
if (_compass) {
// We make the gyro YAW drift correction based on compass magnetic heading
error_course= (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x); // Equation 23, Calculating YAW error
error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x); // Equation 23, Calculating YAW error
} else {
// Use GPS Ground course to correct yaw gyro drift
if (_gps->ground_speed >= SPEEDFILT) {
_course_over_ground_x = cos(ToRad(_gps->ground_course/100.0));
@ -279,7 +284,7 @@ AP_DCM::drift_correction(void)
cos_psi_err = cos(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
// Ryx = sin psi err, Ryy = cos psi err, Ryz = 0
// Ryx = sin psi err, R yy = cos psi err, Ryz = 0
// Rzx = Rzy = 0, Rzz = 1
rot_mat.a.x = cos_psi_err;
rot_mat.a.y = - sin_psi_err;

1
libraries/AP_DCM/AP_DCM.h
View File

@ -43,6 +43,7 @@ public:
Matrix3f get_dcm_matrix(void);
void set_centripetal(bool b);
void set_compass(Compass *compass);
// Methods
void update_DCM(float _G_Dt);

4
libraries/AP_IMU/AP_IMU.cpp
View File

@ -54,15 +54,15 @@ const float AP_IMU::_gyro_temp_curve[3][3] = {
{1665,0,0}
}; // To Do - make additional constructors to pass this in.
void
AP_IMU::init(void)
{
init_gyro();
init_accel();
}
/**************************************************/
void
AP_IMU::init_gyro(void)
{