diff --git a/libraries/AP_IMU/AP_IMU_Oilpan.cpp b/libraries/AP_IMU/AP_IMU_Oilpan.cpp index 6a8f17c2e0..b332142520 100644 --- a/libraries/AP_IMU/AP_IMU_Oilpan.cpp +++ b/libraries/AP_IMU/AP_IMU_Oilpan.cpp @@ -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; diff --git a/libraries/AP_IMU/AP_IMU_Oilpan.h b/libraries/AP_IMU/AP_IMU_Oilpan.h index 90b683e300..5941f0a361 100644 --- a/libraries/AP_IMU/AP_IMU_Oilpan.h +++ b/libraries/AP_IMU/AP_IMU_Oilpan.h @@ -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 // diff --git a/libraries/AP_IMU/IMU.h b/libraries/AP_IMU/IMU.h index 19754fc365..42a128ea58 100644 --- a/libraries/AP_IMU/IMU.h +++ b/libraries/AP_IMU/IMU.h @@ -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