mirror of https://github.com/ArduPilot/ardupilot
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
parent
70bf945d40
commit
8b3d9400e5
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
//
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue