DCM: fixed bug in accel averaging

sorry Randy!
This commit is contained in:
Andrew Tridgell 2012-03-03 20:23:23 +11:00
parent 73594199c8
commit 2d6680f144

View File

@ -78,6 +78,7 @@ AP_DCM::update_DCM_fast(void)
case 2:
_accel_vector = _accel_sum / _accel_sum_count;
_accel_sum_count = 0;
_accel_sum.zero();
drift_correction(); // Normalize the DCM matrix
break;