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