root / quad1 / AeroQuad / DataAcquisition.h @ 9240aaa3
History | View | Annotate | Download (5.35 KB)
1 | 9240aaa3 | Alex | /*
|
---|---|---|---|
2 | AeroQuad v2.1 - October 2010
|
||
3 | www.AeroQuad.com
|
||
4 | Copyright (c) 2010 Ted Carancho. All rights reserved.
|
||
5 | An Open Source Arduino based multicopter.
|
||
6 |
|
||
7 | This program is free software: you can redistribute it and/or modify
|
||
8 | it under the terms of the GNU General Public License as published by
|
||
9 | the Free Software Foundation, either version 3 of the License, or
|
||
10 | (at your option) any later version.
|
||
11 | |||
12 | This program is distributed in the hope that it will be useful,
|
||
13 | but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||
15 | GNU General Public License for more details.
|
||
16 | |||
17 | You should have received a copy of the GNU General Public License
|
||
18 | along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||
19 | */
|
||
20 | |||
21 | // This header file defines function calls and ISR's needed to communicatw
|
||
22 | // over SPI, I2C and other bus communication protocols for measuring sensor data
|
||
23 | |||
24 | // *******************************************
|
||
25 | // SPI Communication for APM ADC
|
||
26 | // Code written by: Jordi Munoz and Jose Julio
|
||
27 | // *******************************************
|
||
28 | #ifdef ArduCopter
|
||
29 | #include <inttypes.h> |
||
30 | #include <avr/interrupt.h> |
||
31 | #include "WConstants.h" |
||
32 | |||
33 | #define bit_set(p,m) ((p) |= (1<<m)) |
||
34 | #define bit_clear(p,m) ((p) &= ~(1<<m)) |
||
35 | |||
36 | // We use Serial Port 2 in SPI Mode
|
||
37 | #define ADC_DATAOUT 51 // MOSI |
||
38 | #define ADC_DATAIN 50 // MISO |
||
39 | #define ADC_SPICLOCK 52 // SCK |
||
40 | #define ADC_CHIP_SELECT 33 // PC4 9 // PH6 Puerto:0x08 Bit mask : 0x40 |
||
41 | |||
42 | // Commands for reading ADC channels on ADS7844
|
||
43 | const unsigned char adc_cmd[9]= { 0x87, 0xC7, 0x97, 0xD7, 0xA7, 0xE7, 0xB7, 0xF7, 0x00 }; |
||
44 | volatile long adc_value[8]; |
||
45 | volatile unsigned char adc_counter[8]; |
||
46 | |||
47 | unsigned char ADC_SPI_transfer(unsigned char data) { |
||
48 | /* Wait for empty transmit buffer */
|
||
49 | while ( !( UCSR2A & (1<<UDRE2)) ); |
||
50 | /* Put data into buffer, sends the data */
|
||
51 | UDR2 = data; |
||
52 | /* Wait for data to be received */
|
||
53 | while ( !(UCSR2A & (1<<RXC2)) ); |
||
54 | /* Get and return received data from buffer */
|
||
55 | return UDR2;
|
||
56 | } |
||
57 | |||
58 | ISR (TIMER2_OVF_vect) { |
||
59 | uint8_t ch; |
||
60 | unsigned int adc_tmp; |
||
61 | |||
62 | //bit_set(PORTL,6); // To test performance
|
||
63 | bit_clear(PORTC,4); // Enable Chip Select (PIN PC4) |
||
64 | ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel |
||
65 | for (ch=0;ch<7;ch++) { |
||
66 | adc_tmp = ADC_SPI_transfer(0)<<8; // Read first byte |
||
67 | adc_tmp |= ADC_SPI_transfer(adc_cmd[ch+1]); // Read second byte and send next command |
||
68 | adc_value[ch] += adc_tmp>>3; // Shift to 12 bits |
||
69 | adc_counter[ch]++; // Number of samples
|
||
70 | } |
||
71 | bit_set(PORTC,4); // Disable Chip Select (PIN PC4) |
||
72 | //bit_clear(PORTL,6); // To test performance
|
||
73 | TCNT2 = 104; // 400 Hz |
||
74 | } |
||
75 | |||
76 | void initialize_ArduCopter_ADC(void) { |
||
77 | unsigned char tmp; |
||
78 | |||
79 | pinMode(ADC_CHIP_SELECT,OUTPUT); |
||
80 | |||
81 | digitalWrite(ADC_CHIP_SELECT,HIGH); // Disable device (Chip select is active low)
|
||
82 | |||
83 | // Setup Serial Port2 in SPI mode
|
||
84 | UBRR2 = 0;
|
||
85 | DDRH |= (1<<PH2); // SPI clock XCK2 (PH2) as output. This enable SPI Master mode |
||
86 | // Set MSPI mode of operation and SPI data mode 0.
|
||
87 | UCSR2C = (1<<UMSEL21)|(1<<UMSEL20); //|(0<<UCPHA2)|(0<<UCPOL2); |
||
88 | // Enable receiver and transmitter.
|
||
89 | UCSR2B = (1<<RXEN2)|(1<<TXEN2); |
||
90 | // Set Baud rate
|
||
91 | UBRR2 = 2; // SPI clock running at 2.6MHz |
||
92 | |||
93 | |||
94 | // Enable Timer2 Overflow interrupt to capture ADC data
|
||
95 | TIMSK2 = 0; // Disable interrupts |
||
96 | TCCR2A = 0; // normal counting mode |
||
97 | TCCR2B = _BV(CS21)|_BV(CS22); // Set prescaler of 256
|
||
98 | TCNT2 = 0;
|
||
99 | TIFR2 = _BV(TOV2); // clear pending interrupts;
|
||
100 | TIMSK2 = _BV(TOIE2) ; // enable the overflow interrupt
|
||
101 | } |
||
102 | |||
103 | int analogRead_ArduCopter_ADC(unsigned char ch_num) { |
||
104 | int result;
|
||
105 | cli(); // We stop interrupts to read the variables
|
||
106 | if (adc_counter[ch_num]>0) |
||
107 | result = adc_value[ch_num]/adc_counter[ch_num]; |
||
108 | else
|
||
109 | result = 0;
|
||
110 | adc_value[ch_num] = 0; // Initialize for next reading |
||
111 | adc_counter[ch_num] = 0;
|
||
112 | sei(); |
||
113 | return(result);
|
||
114 | } |
||
115 | |||
116 | void zero_ArduCopter_ADC(void) { |
||
117 | for (byte n; n<8; n++) { |
||
118 | adc_value[n] = 0;
|
||
119 | adc_counter[n] = 0;
|
||
120 | } |
||
121 | } |
||
122 | #endif
|
||
123 | |||
124 | // ********************************************
|
||
125 | // I2C Communication with Wii Sensors
|
||
126 | // Original code written by lamarche_mathieu
|
||
127 | // Modifications by jihlein
|
||
128 | // ********************************************
|
||
129 | // I2C function calls defined in I2C.h
|
||
130 | |||
131 | short NWMP_acc[3]; |
||
132 | short NWMP_gyro[3]; |
||
133 | |||
134 | void Init_Gyro_acc();
|
||
135 | void updateControls();
|
||
136 | |||
137 | void Init_Gyro_Acc(void) { |
||
138 | //Init WM+ and Nunchuk
|
||
139 | updateRegisterI2C(0x53, 0xFE, 0x05); |
||
140 | delay(100);
|
||
141 | updateRegisterI2C(0x53, 0xF0, 0x55); |
||
142 | delay(100);
|
||
143 | }; |
||
144 | |||
145 | void updateControls() {
|
||
146 | int i,j;
|
||
147 | unsigned char buffer[6]; |
||
148 | |||
149 | for(j=0;j<2;j++) { |
||
150 | sendByteI2C(0x52, 0x00); |
||
151 | Wire.requestFrom(0x52,6); |
||
152 | for(i = 0; i < 6; i++) |
||
153 | buffer[i] = Wire.receive(); |
||
154 | if (buffer[5] & 0x02) { //If WiiMP |
||
155 | NWMP_gyro[0]= (((buffer[4]>>2)<<8) + buffer[1])/16; //hji |
||
156 | NWMP_gyro[1]= (((buffer[5]>>2)<<8) + buffer[2])/16; //hji |
||
157 | NWMP_gyro[2]=-(((buffer[3]>>2)<<8) + buffer[0])/16; //hji |
||
158 | } |
||
159 | else {//If Nunchuk |
||
160 | NWMP_acc[0]=(buffer[2]<<1)|((buffer[5]>>4)&0x01); //hji |
||
161 | NWMP_acc[1]=(buffer[3]<<1)|((buffer[5]>>5)&0x01); //hji |
||
162 | NWMP_acc[2]=buffer[4]; //hji |
||
163 | NWMP_acc[2]=NWMP_acc[2]<<1; //hji |
||
164 | NWMP_acc[2]=NWMP_acc[2] & 0xFFFC; //hji |
||
165 | NWMP_acc[2]=NWMP_acc[2]|((buffer[5]>>6)&0x03); //hji |
||
166 | } |
||
167 | } |
||
168 | } |
||
169 |