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

DCM: range check the matrix before calculating pitch

Browse Source 
The asin() in the pitch calculation can only take values between -1
and 1. This change ensures that the value is in range, and if it isn't
then we force a normalization. If that fails we reset the matrix
This commit is contained in:
Andrew Tridgell 2012-02-23 08:00:25 +11:00
parent 13693e1a04
commit 9caa4aeb44
2 changed files with 31 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_DCM/AP_DCM.cpp
View File

@ -225,6 +225,33 @@ AP_DCM::matrix_reset(void)
}
}
/*
check the DCM matrix for pathological values
*/
void
AP_DCM::check_matrix(void)
{
// some DCM matrix values can lead to an out of range error in
// the pitch calculation via asin(). These NaN values can
// feed back into the rest of the DCM matrix via the
// error_course value.
if (_dcm_matrix.c.x > 1.0 ||
_dcm_matrix.c.x < -1.0) {
// We have an invalid matrix. Force a normalisation.
renorm_range_count++;
normalize();
if (_dcm_matrix.c.x > 1.0 ||
_dcm_matrix.c.x < -1.0) {
// normalisation didn't fix the problem! We're
// in real trouble. All we can do is reset
SITL_debug("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
_dcm_matrix.c.x);
renorm_blowup_count++;
matrix_reset();
}
}
}
/*************************************************
Direction Cosine Matrix IMU: Theory
William Premerlani and Paul Bizard
@ -410,6 +437,8 @@ AP_DCM::drift_correction(void)
void
AP_DCM::euler_angles(void)
{
check_matrix();
#if (OUTPUTMODE == 2) // Only accelerometer info (debugging purposes)
roll = atan2(_accel_vector.y, -_accel_vector.z); // atan2(acc_y, acc_z)
pitch = asin((_accel_vector.x) / (double)9.81); // asin(acc_x)
@ -431,6 +460,7 @@ AP_DCM::euler_angles(void)
void
AP_DCM::euler_rp(void)
{
check_matrix();
pitch = -asin(_dcm_matrix.c.x);
roll = atan2(_dcm_matrix.c.y, _dcm_matrix.c.z);
roll_sensor = roll * DEGX100; //degrees(roll) * 100;

1
libraries/AP_DCM/AP_DCM.h
View File

@ -101,6 +101,7 @@ private:
float read_adc(int select);
void matrix_update(float _G_Dt);
void normalize(void);
void check_matrix(void);
Vector3f renorm(Vector3f const &a, int &problem);
void drift_correction(void);
void euler_angles(void);