IMU: re-work the IMU library to take advantage of the ADC Ch6() call

This changes the IMU code to read 6 synchronised ADC channels at one
time, giving us matching values, and exposing the exact averaging time
to callers
This commit is contained in:
Andrew Tridgell 2011-09-15 13:03:18 +10:00
parent 8a31af801f
commit bb35fdec10
3 changed files with 47 additions and 20 deletions

View File

@ -77,6 +77,7 @@ AP_IMU_Oilpan::_init_gyro(void (*callback)(unsigned long t))
float prev[3] = {0,0,0};
float total_change;
float max_offset;
uint16_t adc_values[6];
// cold start
tc_temp = _adc->Ch(_gyro_temp_ch);
@ -88,8 +89,7 @@ AP_IMU_Oilpan::_init_gyro(void (*callback)(unsigned long t))
digitalWrite(C_LED_PIN, HIGH);
callback(20);
for (int i = 0; i < 6; i++)
adc_in = _adc->Ch(_sensors[i]);
_adc->Ch6(_sensors, adc_values);
digitalWrite(A_LED_PIN, HIGH);
digitalWrite(C_LED_PIN, LOW);
@ -100,16 +100,23 @@ AP_IMU_Oilpan::_init_gyro(void (*callback)(unsigned long t))
_sensor_cal[j] = 500; // Just a large value to load prev[j] the first time
do {
// get 6 sensor values
_adc->Ch6(_sensors, adc_values);
for (int j = 0; j <= 2; j++){
prev[j] = _sensor_cal[j];
adc_in = _adc->Ch(_sensors[j]);
adc_in = adc_values[j];
adc_in -= _sensor_compensation(j, tc_temp);
_sensor_cal[j] = adc_in;
}
for(int i = 0; i < 50; i++){
// get 6 sensor values
_adc->Ch6(_sensors, adc_values);
for (int j = 0; j < 3; j++){
adc_in = _adc->Ch(_sensors[j]);
adc_in = adc_values[j];
// Subtract temp compensated typical gyro bias
adc_in -= _sensor_compensation(j, tc_temp);
// filter
@ -159,6 +166,7 @@ AP_IMU_Oilpan::_init_accel(void (*callback)(unsigned long t))
float prev[6] = {0,0,0};
float total_change;
float max_offset;
uint16_t adc_values[6];
// cold start
callback(500);
@ -168,9 +176,11 @@ AP_IMU_Oilpan::_init_accel(void (*callback)(unsigned long t))
for (int j=3; j<=5; j++) _sensor_cal[j] = 500; // Just a large value to load prev[j] the first time
do {
_adc->Ch6(_sensors, adc_values);
for (int j = 3; j <= 5; j++){
prev[j] = _sensor_cal[j];
adc_in = _adc->Ch(_sensors[j]);
adc_in = adc_values[j];
adc_in -= _sensor_compensation(j, 0); // temperature ignored
_sensor_cal[j] = adc_in;
}
@ -179,8 +189,10 @@ AP_IMU_Oilpan::_init_accel(void (*callback)(unsigned long t))
callback(20);
_adc->Ch6(_sensors, adc_values);
for (int j = 3; j < 6; j++){
adc_in = _adc->Ch(_sensors[j]);
adc_in = adc_values[j];
adc_in -= _sensor_compensation(j, 0); // temperature ignored
_sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1;
}
@ -222,7 +234,7 @@ AP_IMU_Oilpan::_sensor_compensation(uint8_t channel, int temperature) const
// do gyro temperature compensation
if (channel < 3) {
return 1658;
return 1658.0;
/*
return _gyro_temp_curve[channel][0] +
_gyro_temp_curve[channel][1] * temperature +
@ -231,17 +243,17 @@ AP_IMU_Oilpan::_sensor_compensation(uint8_t channel, int temperature) const
}
// do fixed-offset accelerometer compensation
return 2041; // Average raw value from a 20 board sample
return 2041.0; // Average raw value from a 20 board sample
}
float
AP_IMU_Oilpan::_sensor_in(uint8_t channel, int temperature)
AP_IMU_Oilpan::_sensor_in(uint8_t channel, uint16_t adc_value, int temperature)
{
float adc_in;
// get the compensated sensor value
//
adc_in = _adc->Ch(_sensors[channel]) - _sensor_compensation(channel, temperature);
adc_in = adc_value - _sensor_compensation(channel, temperature);
// adjust for sensor sign and apply calibration offset
//
@ -265,18 +277,21 @@ bool
AP_IMU_Oilpan::update(void)
{
int tc_temp = _adc->Ch(_gyro_temp_ch);
uint16_t adc_values[6];
_ticks = _adc->Ch6(_sensors, adc_values);
// convert corrected gyro readings to delta acceleration
//
_gyro.x = ToRad(_gyro_gain_x) * _sensor_in(0, tc_temp);
_gyro.y = ToRad(_gyro_gain_y) * _sensor_in(1, tc_temp);
_gyro.z = ToRad(_gyro_gain_z) * _sensor_in(2, tc_temp);
_gyro.x = _gyro_gain_x * _sensor_in(0, adc_values[0], tc_temp);
_gyro.y = _gyro_gain_y * _sensor_in(1, adc_values[1], tc_temp);
_gyro.z = _gyro_gain_z * _sensor_in(2, adc_values[2], tc_temp);
// convert corrected accelerometer readings to acceleration
//
_accel.x = _accel_scale * _sensor_in(3, tc_temp);
_accel.y = _accel_scale * _sensor_in(4, tc_temp);
_accel.z = _accel_scale * _sensor_in(5, tc_temp);
_accel.x = _accel_scale * _sensor_in(3, adc_values[3], tc_temp);
_accel.y = _accel_scale * _sensor_in(4, adc_values[4], tc_temp);
_accel.z = _accel_scale * _sensor_in(5, adc_values[5], tc_temp);
_accel_filtered.x = _accel_filtered.x * .98 + _accel.x * .02;
_accel_filtered.y = _accel_filtered.y * .98 + _accel.y * .02;

View File

@ -68,8 +68,8 @@ private:
virtual void _init_accel(void (*callback)(unsigned long t)); ///< no-save implementation
virtual void _init_gyro(void (*callback)(unsigned long t)); ///< no-save implementation
float _sensor_in(uint8_t channel, uint16_t adc_value, int temperature);
float _sensor_compensation(uint8_t channel, int temp) const;
float _sensor_in(uint8_t channel, int temperature);
// constants
static const uint8_t _sensors[6]; ///< ADC channel mappings for the sensors
@ -83,14 +83,15 @@ private:
//
static const float _gravity = 423.8; ///< 1G in the raw data coming from the accelerometer
// Value based on actual sample data from 20 boards
static const float _accel_scale = 9.80665 / 423.8; ///< would like to use _gravity here, but cannot
// IDG500 Sensitivity (from datasheet) => 2.0mV/degree/s, 0.8mV/ADC step => 0.8/3.33 = 0.4
// Tested values : 0.4026, ?, 0.4192
//
static const float _gyro_gain_x = 0.4; // X axis Gyro gain
static const float _gyro_gain_y = 0.41; // Y axis Gyro gain
static const float _gyro_gain_z = 0.41; // Z axis Gyro gain
static const float _gyro_gain_x = ToRad(0.4); // X axis Gyro gain
static const float _gyro_gain_y = ToRad(0.41); // Y axis Gyro gain
static const float _gyro_gain_z = ToRad(0.41); // Z axis Gyro gain
// Maximum possible value returned by an offset-corrected sensor channel
//

View File

@ -70,12 +70,19 @@ public:
///
Vector3f get_accel(void) { return _accel; }
/// Fetch the current accelerometer values
///
/// @returns vector of current accelerations in m/s/s
///
Vector3f get_accel_filtered(void) { return _accel_filtered; }
/// return the number of seconds that the last update represents
///
/// @returns number of seconds
///
float get_delta_time(void) { return _ticks * (2.5/1000.0); }
/// A count of bad sensor readings
///
/// @todo This should be renamed, as there's no guarantee that sensors
@ -90,6 +97,10 @@ protected:
/// Most recent gyro reading obtained by ::update
Vector3f _gyro;
/// number of 2.5ms ticks that the accel and gyro values
/// were calculated from
uint16_t _ticks;
};
#endif