change ADC sample rate to 1kHz

this costs us about 9% of our CPU, but should make aliasing much less
of a problem.

libraries/AP_ADC/AP_ADC.h

@@ -33,10 +33,11 @@ class AP_ADC
 	   Pass in an array of 6 channel numbers and results are
 	   returned in result[]
 
-	   The function returns the amount of time that the returned
+	   The function returns the amount of time (in microseconds)
-	   value has been averaged over, in 2.5 millisecond units
+	   since the last call to Ch6().
 	*/
 	virtual uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result) = 0;
+
   private:
 };
 

libraries/AP_ADC/AP_ADC_ADS7844.cpp

@@ -19,9 +19,11 @@
 	XCK2 = SCK = pin PH2
 	Chip Select pin is PC4 (33)	 [PH6 (9)]
 	We are using the 16 clocks per conversion timming to increase efficiency (fast)
-	The sampling frequency is 400Hz (Timer2 overflow interrupt)
+
+	The sampling frequency is 1kHz (Timer2 overflow interrupt)
+
 	So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so
-	we have an 4x oversampling and averaging.
+	we have an 10x oversampling and averaging.
 
 	Methods:
 		Init() : Initialization of interrupts an Timers (Timer2 overflow interrupt)
@@ -60,14 +62,14 @@ static volatile uint32_t _sum[8];
 
 // how many values we've accumulated since last read
 static volatile uint16_t _count[8];
-
+
-static unsigned char ADC_SPI_transfer(unsigned char data)
+static uint32_t last_ch6_micros;
-{
+
-	/* Wait for empty transmit buffer */
+static inline unsigned char ADC_SPI_transfer(unsigned char data)
-	while ( !( UCSR2A & (1 << UDRE2)) );
+{
-	/* Put data into buffer, sends the data */
+	/* Put data into buffer, sends the data */
-	UDR2 = data;
+	UDR2 = data;
-	/* Wait for data to be received */
+	/* Wait for data to be received */
 	while ( !(UCSR2A & (1 << RXC2)) );
 	/* Get and return received data from buffer */
 	return UDR2;
@@ -75,38 +77,39 @@ static unsigned char ADC_SPI_transfer(unsigned char data)
 
 
 ISR (TIMER2_OVF_vect)
-{
+{
-	uint8_t ch;
+	uint8_t ch;
-
+
-	//bit_set(PORTL,6);                        			  	// To test performance
-
 	bit_clear(PORTC, 4);							// Enable Chip Select (PIN PC4)
 	ADC_SPI_transfer(adc_cmd[0]);						// Command to read the first channel
-
+
-	for (ch = 0; ch < 8; ch++){
+	for (ch = 0; ch < 8; ch++) {
-		uint16_t adc_tmp;
+		uint16_t count = _count[ch];
-		adc_tmp  = ADC_SPI_transfer(0) << 8;			// Read first byte
+                uint32_t sum = _sum[ch];
-		adc_tmp |= ADC_SPI_transfer(adc_cmd[ch + 1]);		// Read second byte and send next command
+
-
+		if (++count == 0) {
-		_count[ch]++;
-		if (_count[ch] == 0) {
 			// overflow ... shouldn't happen too often
 			// unless we're just not using the
 			// channel. Notice that we overflow the count
 			// to 1 here, not zero, as otherwise the
 			// reader below could get a division by zero
-			_sum[ch] = 0;
+			sum = 0;
-			_count[ch] = 1;
+			count = 1;
+			last_ch6_micros = micros();
 		}
-		_sum[ch] += adc_tmp;
+		_count[ch] = count;
-	}
+
-
+		sum += ADC_SPI_transfer(0) << 8;	   // Read first byte
+		sum += ADC_SPI_transfer(adc_cmd[ch + 1]);  // Read second byte and send next command
+
+		_sum[ch] = sum;
+	}
+
 	bit_set(PORTC, 4);					// Disable Chip Select (PIN PC4)
-	//bit_clear(PORTL,6);   	                        // To test performance
+	TCNT2 = 200;
-	TCNT2 = 104;						// 400Hz interrupt
+}
-}
+
-
+
-
 // Constructors ////////////////////////////////////////////////////////////////
 AP_ADC_ADS7844::AP_ADC_ADS7844()
 {
@@ -114,21 +117,21 @@ AP_ADC_ADS7844::AP_ADC_ADS7844()
 
 // Public Methods //////////////////////////////////////////////////////////////
 void AP_ADC_ADS7844::Init(void)
-{
+{
-	pinMode(ADC_CHIP_SELECT, OUTPUT);
+	pinMode(ADC_CHIP_SELECT, OUTPUT);
-
+
-	digitalWrite(ADC_CHIP_SELECT, HIGH);								 // Disable device (Chip select is active low)
+	digitalWrite(ADC_CHIP_SELECT, HIGH);  // Disable device (Chip select is active low)
-
+
-	// Setup Serial Port2 in SPI mode
+	// Setup Serial Port2 in SPI mode
-	UBRR2 = 0;
+	UBRR2 = 0;
-	DDRH |= (1 << PH2);																	 // SPI clock XCK2 (PH2) as output. This enable SPI Master mode
+	DDRH |= (1 << PH2);	// SPI clock XCK2 (PH2) as output. This enable SPI Master mode
-	// Set MSPI mode of operation and SPI data mode 0.
+	// Set MSPI mode of operation and SPI data mode 0.
-	UCSR2C = (1 << UMSEL21) | (1 << UMSEL20);								 // |(0 << UCPHA2) | (0 << UCPOL2);
+	UCSR2C = (1 << UMSEL21) | (1 << UMSEL20); // |(0 << UCPHA2) | (0 << UCPOL2);
-	// Enable receiver and transmitter.
+	// Enable receiver and transmitter.
-	UCSR2B = (1 << RXEN2) | (1 << TXEN2);
+	UCSR2B = (1 << RXEN2) | (1 << TXEN2);
-	// Set Baud rate
+	// Set Baud rate
-	UBRR2 = 2;																					// SPI clock running at 2.6MHz
+	UBRR2 = 2;	// SPI clock running at 2.6MHz
-
+
 	// get an initial value for each channel. This ensures
 	// _count[] is never zero
 	for (uint8_t i=0; i<8; i++) {
@@ -139,17 +142,18 @@ void AP_ADC_ADS7844::Init(void)
 		_sum[i]   = adc_tmp;
 	}
 
-
+	last_ch6_micros = micros();
-	// Enable Timer2 Overflow interrupt to capture ADC data
+
-	TIMSK2 = 0;																				 // Disable interrupts
+	// Enable Timer2 Overflow interrupt to capture ADC data
-	TCCR2A = 0;																				 // normal counting mode
+	TIMSK2 = 0;			// Disable interrupts
-	TCCR2B = _BV(CS21) | _BV(CS22);											 // Set prescaler of 256
+	TCCR2A = 0;			// normal counting mode
-	TCNT2	= 0;
+	TCCR2B = _BV(CS21) | _BV(CS22);	// Set prescaler of 256
-	TIFR2	= _BV(TOV2);																 // clear pending interrupts;
+	TCNT2	= 0;
-	TIMSK2 = _BV(TOIE2) ;															 // enable the overflow interrupt
+	TIFR2	= _BV(TOV2);	   // clear pending interrupts;
-}
+	TIMSK2 = _BV(TOIE2) ;	   // enable the overflow interrupt
-
+}
-// Read one channel value
+
+// Read one channel value
 uint16_t AP_ADC_ADS7844::Ch(uint8_t ch_num)
 {
 	uint16_t count;
@@ -174,7 +178,7 @@ uint16_t AP_ADC_ADS7844::Ch(uint8_t ch_num)
 // equal. This will only be true if we always consistently access a
 // sensor by either Ch6() or Ch() and never mix them. If you mix them
 // then you will get very strange results
-uint16_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result)
+uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result)
 {
 	uint16_t count[6];
 	uint32_t sum[6];
@@ -203,7 +207,9 @@ uint16_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result)
 		result[i] = sum[i] / count[i];
 	}
 
-	// this assumes the count in all channels is equal, which will
+	// return number of microseconds since last call
-	// be true if the callers are using the interface consistently
+	uint32_t us = micros();
-	return count[0];
+	uint32_t ret = us - last_ch6_micros;
-}
+	last_ch6_micros = us;
+	return ret;
+}

libraries/AP_ADC/AP_ADC_ADS7844.h

@@ -24,9 +24,9 @@ class AP_ADC_ADS7844 : public AP_ADC
 	uint16_t Ch(unsigned char ch_num);
 
 	// Read 6 sensors at once
-	uint16_t Ch6(const uint8_t *channel_numbers, uint16_t *result);
+	uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result);
-
+
-	private:
+	private:
-};
+};
 
 #endif

libraries/AP_ADC/AP_ADC_HIL.cpp

@@ -47,11 +47,11 @@ void AP_ADC_HIL::Init(void)
 uint16_t AP_ADC_HIL::Ch(unsigned char ch_num)
 {
 	return adcValue[ch_num];
-}
+}
 
 // Read 6 channel values
-uint16_t AP_ADC_HIL::Ch6(const uint8_t *channel_numbers, uint16_t *result)
+uint32_t AP_ADC_HIL::Ch6(const uint8_t *channel_numbers, uint16_t *result)
-{
+{
 	for (uint8_t i=0; i<6; i++) {
 		result[i] = Ch(channel_numbers[i]);
 	}

libraries/AP_ADC/AP_ADC_HIL.h

@@ -34,12 +34,12 @@ class AP_ADC_HIL : public AP_ADC
 	// Read the sensor, part of public AP_ADC interface
 	uint16_t Ch(unsigned char ch_num);
 
-	///
+	///
 	// Read 6 sensors at once
-	uint16_t Ch6(const uint8_t *channel_numbers, uint16_t *result);
+	uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result);
-
+
-	///
+	///
-	// Set the adc raw values given the current rotations rates,
+	// Set the adc raw values given the current rotations rates,
 	// temps, accels, and pressures
 	void setHIL(int16_t p, int16_t q, int16_t r, int16_t gyroTemp,
     	int16_t aX, int16_t aY, int16_t aZ, int16_t diffPress);

libraries/AP_IMU/AP_IMU_Oilpan.cpp

@@ -279,7 +279,7 @@ AP_IMU_Oilpan::update(void)
 	int tc_temp = _adc->Ch(_gyro_temp_ch);
     uint16_t adc_values[6];
 
-    _ticks = _adc->Ch6(_sensors, adc_values);
+    _sample_time = _adc->Ch6(_sensors, adc_values);
 
 	// convert corrected gyro readings to delta acceleration
 	//

libraries/AP_IMU/IMU.h

@@ -81,7 +81,7 @@ public:
 	///
 	/// @returns	number of seconds
 	///
-	float			get_delta_time(void) { return _ticks * (2.5/1000.0); }
+	float			get_delta_time(void) { return _sample_time * 1.0e-6; }
 
 	/// A count of bad sensor readings
 	///
@@ -98,9 +98,9 @@ protected:
 	/// Most recent gyro reading obtained by ::update
 	Vector3f		_gyro;
 
-	/// number of 2.5ms ticks that the accel and gyro values
+	/// number of microseconds that the accel and gyro values
-	/// were calculated from
+	/// were sampled over
-	uint16_t       _ticks;
+	uint32_t       _sample_time;
 };
 
 #endif