From 7e2e78c1af4226ef6ef215f1df72047592b9813b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 5 Aug 2015 15:20:09 +1000 Subject: [PATCH] AP_AHRS: protect against zero deltat in DCM fixes issue #2657 --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index da57a5b909..ecdb465217 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -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; + } } }