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: