aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_AHRS: Update EKF2 data logging

Browse Source
This commit is contained in:
Paul Riseborough 2015-09-24 11:10:11 +10:00 committed by Andrew Tridgell
parent 4acd6c129a
commit f77bdd90fc
1 changed files with 7 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -200,32 +200,25 @@ void AP_AHRS_NavEKF::update_EKF2(void)
}
_gyro_estimate += _gyro_bias;
float abias1, abias2;
EKF2.getAccelZBias(abias1, abias2);
float abias;
EKF2.getAccelZBias(abias);
// This EKF uses the primary IMU
// Eventually we will run a separate instance of the EKF for each IMU and do the selection and blending of EKF outputs upstream
// update _accel_ef_ekf
for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
Vector3f accel = _ins.get_accel(i);
if (i==0) {
accel.z -= abias1;
} else if (i==1) {
accel.z -= abias2;
if (i==_ins.get_primary_accel()) {
accel.z -= abias;
}
if (_ins.get_accel_health(i)) {
_accel_ef_ekf[i] = _dcm_matrix * accel;
}
}
if(_ins.use_accel(0) && _ins.use_accel(1)) {
float IMU1_weighting;
EKF2.getIMU1Weighting(IMU1_weighting);
_accel_ef_ekf_blended = _accel_ef_ekf[0] * IMU1_weighting + _accel_ef_ekf[1] * (1.0f-IMU1_weighting);
} else {
_accel_ef_ekf_blended = _accel_ef_ekf[_ins.get_primary_accel()];
}
}
}
}
// accelerometer values in the earth frame in m/s/s
const Vector3f &AP_AHRS_NavEKF::get_accel_ef(uint8_t i) const