Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
libraries/AP_IMU/AP_IMU_Oilpan.cpp
View File

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

9
libraries/AP_IMU/AP_IMU_Oilpan.h
View File

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

11
libraries/AP_IMU/IMU.h
View File

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