mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 10:28:29 -04:00
AP_InertialSensor: added a get_gyro_drift_rate() interface
this returns the expected max drift rate for the particular type of gyro being used
This commit is contained in:
parent
9e07fa3af6
commit
f46fba54dc
@ -57,6 +57,11 @@ class AP_InertialSensor
|
|||||||
*/
|
*/
|
||||||
virtual uint32_t sample_time() = 0;
|
virtual uint32_t sample_time() = 0;
|
||||||
virtual void reset_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"
|
#include "AP_InertialSensor_Oilpan.h"
|
||||||
|
@ -326,3 +326,11 @@ float AP_InertialSensor_MPU6000::_temp_to_celsius ( uint16_t regval )
|
|||||||
/* TODO */
|
/* TODO */
|
||||||
return 20.0;
|
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);
|
||||||
|
}
|
||||||
|
@ -33,6 +33,7 @@ class AP_InertialSensor_MPU6000 : public AP_InertialSensor
|
|||||||
float temperature();
|
float temperature();
|
||||||
uint32_t sample_time();
|
uint32_t sample_time();
|
||||||
void reset_sample_time();
|
void reset_sample_time();
|
||||||
|
float get_gyro_drift_rate();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
@ -126,3 +126,10 @@ float AP_InertialSensor_Oilpan::_accel_apply_std_offset( float adc_value )
|
|||||||
/* Magic number from AP_ADC_Oilpan.h */
|
/* Magic number from AP_ADC_Oilpan.h */
|
||||||
return ((float) adc_value ) - 2041.0f;
|
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);
|
||||||
|
}
|
||||||
|
@ -32,6 +32,7 @@ class AP_InertialSensor_Oilpan : public AP_InertialSensor
|
|||||||
float temperature();
|
float temperature();
|
||||||
uint32_t sample_time();
|
uint32_t sample_time();
|
||||||
void reset_sample_time();
|
void reset_sample_time();
|
||||||
|
float get_gyro_drift_rate();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user