00001 /* 00002 AP_ADC_ADS7844.cpp - ADC ADS7844 Library for Ardupilot Mega 00003 Code by Jordi Muņoz and Jose Julio. DIYDrones.com 00004 00005 Modified by John Ihlein 6/19/2010 to: 00006 1)Prevent overflow of adc_counter when more than 8 samples collected between reads. Probably 00007 only an issue on initial read of ADC at program start. 00008 2)Reorder analog read order as follows: 00009 p, q, r, ax, ay, az 00010 00011 This library is free software; you can redistribute it and/or 00012 modify it under the terms of the GNU Lesser General Public 00013 License as published by the Free Software Foundation; either 00014 version 2.1 of the License, or (at your option) any later version. 00015 00016 External ADC ADS7844 is connected via Serial port 2 (in SPI mode) 00017 TXD2 = MOSI = pin PH1 00018 RXD2 = MISO = pin PH0 00019 XCK2 = SCK = pin PH2 00020 Chip Select pin is PC4 (33) [PH6 (9)] 00021 We are using the 16 clocks per conversion timming to increase efficiency (fast) 00022 The sampling frequency is 400Hz (Timer2 overflow interrupt) 00023 So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so 00024 we have an 4x oversampling and averaging. 00025 00026 Methods: 00027 Init() : Initialization of interrupts an Timers (Timer2 overflow interrupt) 00028 Ch(ch_num) : Return the ADC channel value 00029 00030 // HJI - Input definitions. USB connector assumed to be on the left, Rx and servo 00031 // connector pins to the rear. IMU shield components facing up. These are board 00032 // referenced sensor inputs, not device referenced. 00033 On Ardupilot Mega Hardware, oriented as described above: 00034 Chennel 0 : yaw rate, r 00035 Channel 1 : roll rate, p 00036 Channel 2 : pitch rate, q 00037 Channel 3 : x/y gyro temperature 00038 Channel 4 : x acceleration, aX 00039 Channel 5 : y acceleration, aY 00040 Channel 6 : z acceleration, aZ 00041 Channel 7 : Differential pressure sensor port 00042 00043 */ 00044 extern "C" { 00045 // AVR LibC Includes 00046 #include <inttypes.h> 00047 #include <avr/interrupt.h> 00048 #include "WConstants.h" 00049 } 00050 00051 #include "AP_ADC_ADS7844.h" 00052 00053 00054 // Commands for reading ADC channels on ADS7844 00055 static const unsigned char adc_cmd[9]= { 0x87, 0xC7, 0x97, 0xD7, 0xA7, 0xE7, 0xB7, 0xF7, 0x00 }; 00056 static volatile long adc_value[8] = { 0, 0, 0, 0, 0, 0, 0, 0 }; 00057 static volatile unsigned char adc_counter[8] = { 0, 0, 0, 0, 0, 0, 0, 0 }; 00058 00059 static unsigned char ADC_SPI_transfer(unsigned char data) 00060 { 00061 /* Wait for empty transmit buffer */ 00062 while ( !( UCSR2A & (1<<UDRE2)) ); 00063 /* Put data into buffer, sends the data */ 00064 UDR2 = data; 00065 /* Wait for data to be received */ 00066 while ( !(UCSR2A & (1<<RXC2)) ); 00067 /* Get and return received data from buffer */ 00068 return UDR2; 00069 } 00070 00071 00072 ISR (TIMER2_OVF_vect) 00073 { 00074 uint8_t ch; 00075 unsigned int adc_tmp; 00076 00077 //bit_set(PORTL,6); // To test performance 00078 bit_clear(PORTC,4); // Enable Chip Select (PIN PC4) 00079 ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel 00080 for (ch=0;ch<8;ch++) 00081 { 00082 if (adc_counter[ch] >= 17) // HJI - Added this to prevent 00083 { // overflow of adc_value 00084 adc_value[ch] = 0; 00085 adc_counter[ch] = 0; 00086 } 00087 adc_tmp = ADC_SPI_transfer(0)<<8; // Read first byte 00088 adc_tmp |= ADC_SPI_transfer(adc_cmd[ch+1]); // Read second byte and send next command 00089 adc_value[ch] += adc_tmp>>3; // Shift to 12 bits 00090 adc_counter[ch]++; // Number of samples 00091 } 00092 bit_set(PORTC,4); // Disable Chip Select (PIN PC4) 00093 //bit_clear(PORTL,6); // To test performance 00094 TCNT2 = 104; // 400 Hz 00095 } 00096 00097 00098 // Constructors //////////////////////////////////////////////////////////////// 00099 AP_ADC_ADS7844::AP_ADC_ADS7844() 00100 { 00101 } 00102 00103 // Public Methods ////////////////////////////////////////////////////////////// 00104 void AP_ADC_ADS7844::Init(void) 00105 { 00106 unsigned char tmp; 00107 00108 pinMode(ADC_CHIP_SELECT,OUTPUT); 00109 00110 digitalWrite(ADC_CHIP_SELECT,HIGH); // Disable device (Chip select is active low) 00111 00112 // Setup Serial Port2 in SPI mode 00113 UBRR2 = 0; 00114 DDRH |= (1<<PH2); // SPI clock XCK2 (PH2) as output. This enable SPI Master mode 00115 // Set MSPI mode of operation and SPI data mode 0. 00116 UCSR2C = (1<<UMSEL21)|(1<<UMSEL20); //|(0<<UCPHA2)|(0<<UCPOL2); 00117 // Enable receiver and transmitter. 00118 UCSR2B = (1<<RXEN2)|(1<<TXEN2); 00119 // Set Baud rate 00120 UBRR2 = 2; // SPI clock running at 2.6MHz 00121 00122 00123 // Enable Timer2 Overflow interrupt to capture ADC data 00124 TIMSK2 = 0; // Disable interrupts 00125 TCCR2A = 0; // normal counting mode 00126 TCCR2B = _BV(CS21)|_BV(CS22); // Set prescaler of 256 00127 TCNT2 = 0; 00128 TIFR2 = _BV(TOV2); // clear pending interrupts; 00129 TIMSK2 = _BV(TOIE2) ; // enable the overflow interrupt 00130 } 00131 00132 // Read one channel value 00133 int AP_ADC_ADS7844::Ch(unsigned char ch_num) 00134 { 00135 int result; 00136 00137 cli(); // We stop interrupts to read the variables 00138 if (adc_counter[ch_num]>0) 00139 result = adc_value[ch_num]/adc_counter[ch_num]; 00140 else 00141 result = 0; 00142 adc_value[ch_num] = 0; // Initialize for next reading 00143 adc_counter[ch_num] = 0; 00144 sei(); 00145 return(result); 00146 }