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

AP_AHRS: only use enabled IMUs in DCM

Browse Source 
honor the INS_USE parameters for DCM
This commit is contained in:
Andrew Tridgell 2019-07-05 12:35:51 +10:00
parent 7a9203ef29
commit 1f309e8401
1 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -150,7 +150,7 @@ AP_AHRS_DCM::matrix_update(float _G_Dt)
Vector3f delta_angle;
const AP_InertialSensor &_ins = AP::ins();
for (uint8_t i=0; i<_ins.get_gyro_count(); i++) {
if (_ins.get_gyro_health(i) && healthy_count < 2) {
if (_ins.use_gyro(i) && healthy_count < 2) {
Vector3f dangle;
if (_ins.get_delta_angle(i, dangle)) {
healthy_count++;
@ -636,8 +636,9 @@ AP_AHRS_DCM::drift_correction(float deltat)
const AP_InertialSensor &_ins = AP::ins();
// rotate accelerometer values into the earth frame
uint8_t healthy_count = 0;
for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
if (_ins.get_accel_health(i)) {
if (_ins.use_accel(i) && healthy_count < 2) {
/*
by using get_delta_velocity() instead of get_accel() the
accel value is sampled over the right time delta for
@ -651,6 +652,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
// integrate the accel vector in the earth frame between GPS readings
_ra_sum[i] += _accel_ef[i] * deltat;
}
healthy_count++;
}
}