Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 23:43:58 -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;
// 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
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
const Vector3f get_gyro(void) const {
const Vector3f &get_gyro(void) const {
return _omega;
}

46
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -26,31 +26,12 @@
extern const AP_HAL::HAL& hal;
// 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()) {
return AP_AHRS_DCM::get_gyro();
}
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;
return _gyro_estimate;
}
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
{
return AP_AHRS_DCM::get_gyro_drift();
if (!using_EKF()) {
return AP_AHRS_DCM::get_gyro_drift();
}
return _gyro_bias;
}
void AP_AHRS_NavEKF::update(void)
@ -100,6 +84,24 @@ void AP_AHRS_NavEKF::update(void)
if (yaw_sensor < 0)
yaw_sensor += 36000;
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
const Vector3f get_gyro(void) const;
const Vector3f &get_gyro(void) const;
const Matrix3f &get_dcm_matrix(void) const;
// return the current drift correction integrator value
@ -102,6 +102,8 @@ private:
bool ekf_started;
Matrix3f _dcm_matrix;
Vector3f _dcm_attitude;
Vector3f _gyro_bias;
Vector3f _gyro_estimate;
const uint16_t startup_delay_ms;
uint32_t start_time_ms;
};