Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AHRS: include P term in omega 

thanks to Jon for suggesting this
This commit is contained in:
Andrew Tridgell 2012-06-27 14:15:16 +10:00
parent 0fa1f29cf3
commit ce016b5ae8
2 changed files with 6 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -77,11 +77,9 @@ void
AP_AHRS_DCM::matrix_update(float _G_Dt) AP_AHRS_DCM::matrix_update(float _G_Dt)
{ {
// Equation 16, adding proportional and integral correction terms // Equation 16, adding proportional and integral correction terms
_omega = _gyro_vector + _omega_I; _omega = _gyro_vector + _omega_I + _omega_P + _omega_yaw_P;
// add in P correction terms _dcm_matrix.rotate(_omega * _G_Dt);
Vector3f r = (_omega + _omega_P + _omega_yaw_P) * _G_Dt;
_dcm_matrix.rotate(r);
} }
@ -95,6 +93,7 @@ AP_AHRS_DCM::reset(bool recover_eulers)
// reset the integration terms // reset the integration terms
_omega_I.zero(); _omega_I.zero();
_omega_P.zero(); _omega_P.zero();
_omega_yaw_P.zero();
_omega.zero(); _omega.zero();
// if the caller wants us to try to recover to the current // if the caller wants us to try to recover to the current
@ -279,7 +278,7 @@ AP_AHRS_DCM::_P_gain(float spin_rate)
// this function prodoces the _omega_yaw_P vector, and also // this function prodoces the _omega_yaw_P vector, and also
// contributes to the _omega_I.z long term yaw drift estimate // contributes to the _omega_I.z long term yaw drift estimate
void void
AP_AHRS_DCM::drift_correction_yaw(float deltat) AP_AHRS_DCM::drift_correction_yaw(void)
{ {
bool new_value = false; bool new_value = false;
float yaw_error; float yaw_error;
@ -370,7 +369,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
// perform yaw drift correction if we have a new yaw reference // perform yaw drift correction if we have a new yaw reference
// vector // vector
drift_correction_yaw(deltat); drift_correction_yaw();
// integrate the accel vector in the earth frame between GPS readings // integrate the accel vector in the earth frame between GPS readings
_ra_sum += _dcm_matrix * (_accel_vector * deltat); _ra_sum += _dcm_matrix * (_accel_vector * deltat);

2
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -57,7 +57,7 @@ private:
void check_matrix(void); void check_matrix(void);
bool renorm(Vector3f const &a, Vector3f &result); bool renorm(Vector3f const &a, Vector3f &result);
void drift_correction(float deltat); void drift_correction(float deltat);
void drift_correction_yaw(float deltat); void drift_correction_yaw(void);
float yaw_error_compass(); float yaw_error_compass();
float yaw_error_gps(); float yaw_error_gps();
void euler_angles(void); void euler_angles(void);