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

AP_AHRS: fixed gyro_bias sign, and pre-calculate gyro_estimate for EKF

Browse Source 
this allows us to return a constant vector for the corrected gyro
estimate. Based on discussions with Jon Challinger
This commit is contained in:
Andrew Tridgell 2014-07-13 21:56:39 +10:00
parent 966d66ef40
commit 28fedba4d8
4 changed files with 29 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_AHRS/AP_AHRS.h
View File

@ -163,7 +163,7 @@ public:
int32_t yaw_sensor; int32_t yaw_sensor;
// return a smoothed and corrected gyro vector // return a smoothed and corrected gyro vector
virtual const Vector3f get_gyro(void) const = 0; virtual const Vector3f &get_gyro(void) const = 0;
// return the current estimate of the gyro drift // return the current estimate of the gyro drift
virtual const Vector3f &get_gyro_drift(void) const = 0; virtual const Vector3f &get_gyro_drift(void) const = 0;

2
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -39,7 +39,7 @@ public:
} }
// return the smoothed gyro vector corrected for drift // return the smoothed gyro vector corrected for drift
const Vector3f get_gyro(void) const { const Vector3f &get_gyro(void) const {
return _omega; return _omega;
} }

44
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -26,31 +26,12 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
// return the smoothed gyro vector corrected for drift // return the smoothed gyro vector corrected for drift
const Vector3f AP_AHRS_NavEKF::get_gyro(void) const const Vector3f &AP_AHRS_NavEKF::get_gyro(void) const
{ {
if (!using_EKF()) { if (!using_EKF()) {
return AP_AHRS_DCM::get_gyro(); return AP_AHRS_DCM::get_gyro();
} }
return _gyro_estimate;
Vector3f angRate;
// average the available gyro sensors
angRate.zero();
uint8_t gyro_count = 0;
for (uint8_t i = 0; i<_ins.get_gyro_count(); i++) {
if (_ins.get_gyro_health(i)) {
angRate += _ins.get_gyro(i);
gyro_count++;
}
}
if (gyro_count != 0) {
angRate /= gyro_count;
}
Vector3f gyrobias;
EKF.getGyroBias(gyrobias);
return angRate+gyrobias;
} }
const Matrix3f &AP_AHRS_NavEKF::get_dcm_matrix(void) const const Matrix3f &AP_AHRS_NavEKF::get_dcm_matrix(void) const
@ -63,7 +44,10 @@ const Matrix3f &AP_AHRS_NavEKF::get_dcm_matrix(void) const
const Vector3f &AP_AHRS_NavEKF::get_gyro_drift(void) const const Vector3f &AP_AHRS_NavEKF::get_gyro_drift(void) const
{ {
if (!using_EKF()) {
return AP_AHRS_DCM::get_gyro_drift(); return AP_AHRS_DCM::get_gyro_drift();
}
return _gyro_bias;
} }
void AP_AHRS_NavEKF::update(void) void AP_AHRS_NavEKF::update(void)
@ -100,6 +84,24 @@ void AP_AHRS_NavEKF::update(void)
if (yaw_sensor < 0) if (yaw_sensor < 0)
yaw_sensor += 36000; yaw_sensor += 36000;
update_trig(); update_trig();
// keep _gyro_bias for get_gyro_drift()
EKF.getGyroBias(_gyro_bias);
_gyro_bias = -_gyro_bias;
// calculate corrected gryo estimate for get_gyro()
_gyro_estimate.zero();
uint8_t healthy_count = 0;
for (uint8_t i=0; i<_ins.get_gyro_count(); i++) {
if (_ins.get_gyro_health(i)) {
_gyro_estimate += _ins.get_gyro(i);
healthy_count++;
}
}
if (healthy_count > 1) {
_gyro_estimate /= healthy_count;
}
_gyro_estimate += _gyro_bias;
} }
} }
} }

4
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -42,7 +42,7 @@ public:
} }
// return the smoothed gyro vector corrected for drift // return the smoothed gyro vector corrected for drift
const Vector3f get_gyro(void) const; const Vector3f &get_gyro(void) const;
const Matrix3f &get_dcm_matrix(void) const; const Matrix3f &get_dcm_matrix(void) const;
// return the current drift correction integrator value // return the current drift correction integrator value
@ -102,6 +102,8 @@ private:
bool ekf_started; bool ekf_started;
Matrix3f _dcm_matrix; Matrix3f _dcm_matrix;
Vector3f _dcm_attitude; Vector3f _dcm_attitude;
Vector3f _gyro_bias;
Vector3f _gyro_estimate;
const uint16_t startup_delay_ms; const uint16_t startup_delay_ms;
uint32_t start_time_ms; uint32_t start_time_ms;
}; };