Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: protect against zero deltat in DCM 

fixes issue #2657
This commit is contained in:
Andrew Tridgell 2015-08-05 15:20:09 +10:00 committed by Randy Mackay
parent 34a5c46bfd
commit 9fbd739ebe
1 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -119,9 +119,11 @@ AP_AHRS_DCM::matrix_update(float _G_Dt)
if (healthy_count > 1) {
delta_angle /= healthy_count;
}
_omega = delta_angle / _G_Dt;
_omega += _omega_I;
_dcm_matrix.rotate((_omega + _omega_P + _omega_yaw_P) * _G_Dt);
if (_G_Dt > 0) {
_omega = delta_angle / _G_Dt;
_omega += _omega_I;
_dcm_matrix.rotate((_omega + _omega_P + _omega_yaw_P) * _G_Dt);
}
}
@ -559,9 +561,11 @@ AP_AHRS_DCM::drift_correction(float deltat)
float delta_velocity_dt;
_ins.get_delta_velocity(i, delta_velocity);
delta_velocity_dt = _ins.get_delta_velocity_dt(i);
_accel_ef[i] = _dcm_matrix * (delta_velocity / delta_velocity_dt);
// integrate the accel vector in the earth frame between GPS readings
_ra_sum[i] += _accel_ef[i] * deltat;
if (delta_velocity_dt > 0) {
_accel_ef[i] = _dcm_matrix * (delta_velocity / delta_velocity_dt);
// integrate the accel vector in the earth frame between GPS readings
_ra_sum[i] += _accel_ef[i] * deltat;
}
}
}