Browse Source

AHRS: disable barometer for vertical acceleration

this seems to have been the cause of the 'flips' seen by Marco and
others. Testing by Craig and Alan shows that the flips are gone when
the barometric acceleration is removed.

It looks like a 5 point average filter is not enough to keep the
vertical acceleteration noise low. With high noise in the z axes, the
x and y axes are scaled back when the ge vector is normalised.
mission-4.1.18
Andrew Tridgell 13 years ago
parent
commit
37fc6c46b9
  1. 8
      libraries/AP_AHRS/AP_AHRS_DCM.cpp

8
libraries/AP_AHRS/AP_AHRS_DCM.cpp

@ -411,11 +411,17 @@ AP_AHRS_DCM::drift_correction(float deltat)
last_correction_time = _gps->last_fix_time; last_correction_time = _gps->last_fix_time;
} }
// even without GPS lock we can correct for the vertical acceleration #if 0
/*
NOTE: The barometric vertical acceleration correction is disabled
until we work out how to filter it sufficiently to be usable
on ArduCopter
*/
if (_barometer != NULL) { if (_barometer != NULL) {
// Z velocity is down // Z velocity is down
velocity.z = - _barometer->get_climb_rate(); velocity.z = - _barometer->get_climb_rate();
} }
#endif
// see if this is our first time through - in which case we // see if this is our first time through - in which case we
// just setup the start times and return // just setup the start times and return

Loading…
Cancel
Save