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

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;

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
 	//

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