From f46fba54dc78cee181b2446acee49474e0c82b4c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 8 Mar 2012 18:10:27 +1100
Subject: [PATCH] AP_InertialSensor: added a get_gyro_drift_rate() interface

this returns the expected max drift rate for the particular type of
gyro being used
---
 libraries/AP_InertialSensor/AP_InertialSensor.h           | 5 +++++
 libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp | 8 ++++++++
 libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h   | 1 +
 libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp  | 7 +++++++
 libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h    | 1 +
 5 files changed, 22 insertions(+)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 08067b8e1d..44907652c1 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -57,6 +57,11 @@ class AP_InertialSensor
    */
   virtual uint32_t sample_time() = 0;
   virtual void reset_sample_time() = 0;
+
+  // return the maximum gyro drift rate in radians/s/s. This
+  // depends on what gyro chips are being used
+  virtual float get_gyro_drift_rate(void) = 0;
+
 };
 
 #include "AP_InertialSensor_Oilpan.h"
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
index 500bfe464d..fd272c72b0 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
@@ -326,3 +326,11 @@ float AP_InertialSensor_MPU6000::_temp_to_celsius ( uint16_t regval )
     /* TODO */
     return 20.0;
 }
+
+// return the MPU6k gyro drift rate in radian/s/s
+// note that this is much better than the oilpan gyros
+float AP_InertialSensor_MPU6000::get_gyro_drift_rate(void)
+{
+    // 0.5 degrees/second/minute
+    return ToRad(0.5/60);
+}
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
index fe48070ca8..35f28ac871 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
@@ -33,6 +33,7 @@ class AP_InertialSensor_MPU6000 : public AP_InertialSensor
   float temperature();
   uint32_t sample_time();
   void reset_sample_time();
+  float get_gyro_drift_rate();
 
   private:
 
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
index 4d13729722..4d2fcb03f4 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
@@ -126,3 +126,10 @@ float AP_InertialSensor_Oilpan::_accel_apply_std_offset( float adc_value )
   /* Magic number from AP_ADC_Oilpan.h */
   return ((float) adc_value ) - 2041.0f;
 }
+
+// return the oilpan gyro drift rate in radian/s/s
+float AP_InertialSensor_Oilpan::get_gyro_drift_rate(void)
+{
+    // 3.0 degrees/second/minute
+    return ToRad(3.0/60);
+}
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h
index 099cde265e..383bdea7f2 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h
@@ -32,6 +32,7 @@ class AP_InertialSensor_Oilpan : public AP_InertialSensor
   float temperature();
   uint32_t sample_time();
   void reset_sample_time();
+  float get_gyro_drift_rate();
 
   private: