Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 17:03:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

DCM: added matrix_reset() method

Browse Source
This commit is contained in:
Andrew Tridgell 2011-12-13 21:32:50 +11:00
parent b0aa1deac2
commit 621f21e4f5
2 changed files with 22 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_DCM/AP_DCM.cpp
View File

@ -188,6 +188,26 @@ AP_DCM::accel_adjust(void)
_accel_vector -= temp; _accel_vector -= temp;
} }
/*
reset the DCM matrix and omega. Used on ground start, and on
extreme errors in the matrix
*/
void
AP_DCM::matrix_reset(void)
{
_dcm_matrix.a.x = 1.0f;
_dcm_matrix.a.y = 0.0f;
_dcm_matrix.a.z = 0.0f;
_dcm_matrix.b.x = 0.0f;
_dcm_matrix.b.y = 1.0f;
_dcm_matrix.b.z = 0.0f;
_dcm_matrix.c.x = 0.0f;
_dcm_matrix.c.y = 0.0f;
_dcm_matrix.c.z = 1.0f;
_omega_I.x = 0.0f;
_omega_I.y = 0.0f;
_omega_I.z = 0.0f;
}
/************************************************* /*************************************************
Direction Cosine Matrix IMU: Theory Direction Cosine Matrix IMU: Theory
@ -221,18 +241,7 @@ AP_DCM::normalize(void)
_dcm_matrix.c = renorm(temporary[2], problem); _dcm_matrix.c = renorm(temporary[2], problem);
if (problem == 1) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down! if (problem == 1) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
_dcm_matrix.a.x = 1.0f; matrix_reset();
_dcm_matrix.a.y = 0.0f;
_dcm_matrix.a.z = 0.0f;
_dcm_matrix.b.x = 0.0f;
_dcm_matrix.b.y = 1.0f;
_dcm_matrix.b.z = 0.0f;
_dcm_matrix.c.x = 0.0f;
_dcm_matrix.c.y = 0.0f;
_dcm_matrix.c.z = 1.0f;
_omega_I.x = 0.0f;
_omega_I.y = 0.0f;
_omega_I.z = 0.0f;
} }
} }

1
libraries/AP_DCM/AP_DCM.h
View File

@ -54,6 +54,7 @@ public:
// Methods // Methods
void update_DCM(void); void update_DCM(void);
void update_DCM_fast(void); void update_DCM_fast(void);
void matrix_reset(void);
long roll_sensor; // Degrees * 100 long roll_sensor; // Degrees * 100
long pitch_sensor; // Degrees * 100 long pitch_sensor; // Degrees * 100