Browse Source

Quaternion: use gyro drift value from sensor driver

master
Andrew Tridgell 13 years ago
parent
commit
eff6778515
  1. 4
      libraries/AP_Quaternion/AP_Quaternion.h

4
libraries/AP_Quaternion/AP_Quaternion.h

@ -25,6 +25,10 @@ public: @@ -25,6 +25,10 @@ public:
b_x = 0;
b_z = -1;
// limit the drift to the drift rate reported by the
// sensor driver
gyroMeasDrift = imu->get_gyro_drift_rate();
// scaled gyro drift limits
beta = sqrt(3.0f / 4.0f) * gyroMeasError;
zeta = sqrt(3.0f / 4.0f) * gyroMeasDrift;

Loading…
Cancel
Save