Browse Source

AP_InertialSensor: added a get_gyro_drift_rate() interface

this returns the expected max drift rate for the particular type of
gyro being used
mission-4.1.18
Andrew Tridgell 13 years ago
parent
commit
9296ac494d
  1. 5
      libraries/AP_InertialSensor/AP_InertialSensor.h
  2. 8
      libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
  3. 1
      libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
  4. 7
      libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
  5. 1
      libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h

5
libraries/AP_InertialSensor/AP_InertialSensor.h

@ -57,6 +57,11 @@ class AP_InertialSensor @@ -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"

8
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@ -326,3 +326,11 @@ float AP_InertialSensor_MPU6000::_temp_to_celsius ( uint16_t regval ) @@ -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);
}

1
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h

@ -33,6 +33,7 @@ class AP_InertialSensor_MPU6000 : public AP_InertialSensor @@ -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:

7
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp

@ -126,3 +126,10 @@ float AP_InertialSensor_Oilpan::_accel_apply_std_offset( float adc_value ) @@ -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);
}

1
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h

@ -32,6 +32,7 @@ class AP_InertialSensor_Oilpan : public AP_InertialSensor @@ -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:

Loading…
Cancel
Save