Rev 945 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 945 | Rev 957 | ||
---|---|---|---|
1 | /*############################################################################ |
1 | /*############################################################################ |
2 | ############################################################################*/ |
2 | ############################################################################*/ |
3 | 3 | ||
4 | #include <avr/io.h> |
4 | #include <avr/io.h> |
5 | #include <avr/interrupt.h> |
5 | #include <avr/interrupt.h> |
6 | 6 | ||
7 | #include "main.h" |
7 | #include "main.h" |
8 | #include "twimaster.h" |
8 | #include "twimaster.h" |
9 | #include "fc.h" |
9 | #include "fc.h" |
10 | #include "analog.h" |
10 | #include "analog.h" |
11 | 11 | ||
12 | volatile uint8_t twi_state = 0; |
12 | volatile uint8_t twi_state = 0; |
13 | volatile uint8_t motor_write = 0; |
13 | volatile uint8_t motor_write = 0; |
14 | volatile uint8_t motor_read = 0; |
14 | volatile uint8_t motor_read = 0; |
15 | volatile uint8_t dac_channel = 0; |
15 | volatile uint8_t dac_channel = 0; |
16 | volatile uint8_t motor_rx[MOTOR_COUNT*2]; |
16 | volatile uint8_t motor_rx[MOTOR_COUNT*2]; |
17 | volatile uint16_t I2CTimeout = 100; |
17 | volatile uint16_t I2CTimeout = 100; |
18 | 18 | ||
19 | 19 | ||
20 | #define SCL_CLOCK 200000L |
20 | #define SCL_CLOCK 200000L |
21 | #define I2C_TIMEOUT 30000 |
21 | #define I2C_TIMEOUT 30000 |
22 | 22 | ||
23 | #define TWSR_STATUS_MASK 0xF8 |
23 | #define TWSR_STATUS_MASK 0xF8 |
24 | // for Master Transmitter Mode |
24 | // for Master Transmitter Mode |
25 | 25 | ||
26 | #define I2C_STATUS_START 0x08 |
26 | #define I2C_STATUS_START 0x08 |
27 | #define I2C_STATUS_REPEATSTART 0x10 |
27 | #define I2C_STATUS_REPEATSTART 0x10 |
28 | #define I2C_STATUS_TX_SLA_ACK 0x18 |
28 | #define I2C_STATUS_TX_SLA_ACK 0x18 |
29 | #define I2C_STATUS_SLAW_NOACK 0x20 |
29 | #define I2C_STATUS_SLAW_NOACK 0x20 |
30 | #define I2C_STATUS_TX_DATA_ACK 0x28 |
30 | #define I2C_STATUS_TX_DATA_ACK 0x28 |
31 | #define I2C_STATUS_TX_DATA_NOTACK 0x30 |
31 | #define I2C_STATUS_TX_DATA_NOTACK 0x30 |
32 | #define I2C_STATUS_RX_DATA_ACK 0x50 |
32 | #define I2C_STATUS_RX_DATA_ACK 0x50 |
33 | #define I2C_STATUS_RX_DATA_NOTACK 0x58 |
33 | #define I2C_STATUS_RX_DATA_NOTACK 0x58 |
34 | 34 | ||
35 | /**************************************************/ |
35 | /**************************************************/ |
36 | /* Initialize I2C (TWI) */ |
36 | /* Initialize I2C (TWI) */ |
37 | /**************************************************/ |
37 | /**************************************************/ |
38 | void I2C_Init(void) |
38 | void I2C_Init(void) |
39 | { |
39 | { |
40 | uint8_t sreg = SREG; |
40 | uint8_t sreg = SREG; |
41 | cli(); |
41 | cli(); |
42 | 42 | ||
43 | // SDA is INPUT |
43 | // SDA is INPUT |
44 | DDRC &= ~(1<<DDC1); |
44 | DDRC &= ~(1<<DDC1); |
45 | // SCL is output |
45 | // SCL is output |
46 | DDRC |= (1<<DDC0); |
46 | DDRC |= (1<<DDC0); |
47 | // pull up SDA |
47 | // pull up SDA |
48 | PORTC |= (1<<PORTC0)|(1<<PORTC1); |
48 | PORTC |= (1<<PORTC0)|(1<<PORTC1); |
49 | 49 | ||
50 | // TWI Status Register |
50 | // TWI Status Register |
51 | // prescaler 1 (TWPS1 = 0, TWPS0 = 0) |
51 | // prescaler 1 (TWPS1 = 0, TWPS0 = 0) |
52 | TWSR &= ~((1<<TWPS1)|(1<<TWPS0)); |
52 | TWSR &= ~((1<<TWPS1)|(1<<TWPS0)); |
53 | 53 | ||
54 | // set TWI Bit Rate Register |
54 | // set TWI Bit Rate Register |
55 | TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
55 | TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
56 | 56 | ||
57 | twi_state = 0; |
57 | twi_state = 0; |
58 | motor_write = 0; |
58 | motor_write = 0; |
59 | motor_read = 0; |
59 | motor_read = 0; |
60 | 60 | ||
61 | SREG = sreg; |
61 | SREG = sreg; |
62 | } |
62 | } |
63 | 63 | ||
64 | /****************************************/ |
64 | /****************************************/ |
65 | /* Start I2C */ |
65 | /* Start I2C */ |
66 | /****************************************/ |
66 | /****************************************/ |
67 | void I2C_Start(void) |
67 | void I2C_Start(void) |
68 | { |
68 | { |
69 | // TWI Control Register |
69 | // TWI Control Register |
70 | // clear TWI interrupt flag (TWINT=1) |
70 | // clear TWI interrupt flag (TWINT=1) |
71 | // disable TWI Acknowledge Bit (TWEA = 0) |
71 | // disable TWI Acknowledge Bit (TWEA = 0) |
72 | // enable TWI START Condition Bit (TWSTA = 1), MASTER |
72 | // enable TWI START Condition Bit (TWSTA = 1), MASTER |
73 | // disable TWI STOP Condition Bit (TWSTO = 0) |
73 | // disable TWI STOP Condition Bit (TWSTO = 0) |
74 | // disable TWI Write Collision Flag (TWWC = 0) |
74 | // disable TWI Write Collision Flag (TWWC = 0) |
75 | // enable i2c (TWEN = 1) |
75 | // enable i2c (TWEN = 1) |
76 | // enable TWI Interrupt (TWIE = 1) |
76 | // enable TWI Interrupt (TWIE = 1) |
77 | TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWIE); |
77 | TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWIE); |
78 | } |
78 | } |
79 | 79 | ||
80 | /****************************************/ |
80 | /****************************************/ |
81 | /* Stop I2C */ |
81 | /* Stop I2C */ |
82 | /****************************************/ |
82 | /****************************************/ |
83 | void I2C_Stop(void) |
83 | void I2C_Stop(void) |
84 | { |
84 | { |
85 | // TWI Control Register |
85 | // TWI Control Register |
86 | // clear TWI interrupt flag (TWINT=1) |
86 | // clear TWI interrupt flag (TWINT=1) |
87 | // disable TWI Acknowledge Bit (TWEA = 0) |
87 | // disable TWI Acknowledge Bit (TWEA = 0) |
88 | // diable TWI START Condition Bit (TWSTA = 1), no MASTER |
88 | // diable TWI START Condition Bit (TWSTA = 1), no MASTER |
89 | // enable TWI STOP Condition Bit (TWSTO = 1) |
89 | // enable TWI STOP Condition Bit (TWSTO = 1) |
90 | // disable TWI Write Collision Flag (TWWC = 0) |
90 | // disable TWI Write Collision Flag (TWWC = 0) |
91 | // enable i2c (TWEN = 1) |
91 | // enable i2c (TWEN = 1) |
92 | // disable TWI Interrupt (TWIE = 0) |
92 | // disable TWI Interrupt (TWIE = 0) |
93 | TWCR = (1<<TWINT) | (1<<TWSTO) | (1<<TWEN); |
93 | TWCR = (1<<TWINT) | (1<<TWSTO) | (1<<TWEN); |
94 | } |
94 | } |
95 | 95 | ||
96 | 96 | ||
97 | /****************************************/ |
97 | /****************************************/ |
98 | /* Write to I2C */ |
98 | /* Write to I2C */ |
99 | /****************************************/ |
99 | /****************************************/ |
100 | void I2C_WriteByte(int8_t byte) |
100 | void I2C_WriteByte(int8_t byte) |
101 | { |
101 | { |
102 | // move byte to send into TWI Data Register |
102 | // move byte to send into TWI Data Register |
103 | TWDR = byte; |
103 | TWDR = byte; |
104 | // clear interrupt flag (TWINT = 1) |
104 | // clear interrupt flag (TWINT = 1) |
105 | // enable i2c bus (TWEN = 1) |
105 | // enable i2c bus (TWEN = 1) |
106 | // enable interrupt (TWIE = 1) |
106 | // enable interrupt (TWIE = 1) |
107 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
107 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
108 | } |
108 | } |
109 | 109 | ||
110 | 110 | ||
111 | /****************************************/ |
111 | /****************************************/ |
112 | /* Receive byte and send ACK */ |
112 | /* Receive byte and send ACK */ |
113 | /****************************************/ |
113 | /****************************************/ |
114 | void I2C_ReceiveByte(void) |
114 | void I2C_ReceiveByte(void) |
115 | { |
115 | { |
116 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA); |
116 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA); |
117 | } |
117 | } |
118 | 118 | ||
119 | /****************************************/ |
119 | /****************************************/ |
120 | /* I2C receive last byte and send no ACK*/ |
120 | /* I2C receive last byte and send no ACK*/ |
121 | /****************************************/ |
121 | /****************************************/ |
122 | void I2C_ReceiveLastByte(void) |
122 | void I2C_ReceiveLastByte(void) |
123 | { |
123 | { |
124 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
124 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
125 | } |
125 | } |
126 | 126 | ||
127 | 127 | ||
128 | /****************************************/ |
128 | /****************************************/ |
129 | /* Reset I2C */ |
129 | /* Reset I2C */ |
130 | /****************************************/ |
130 | /****************************************/ |
131 | void I2C_Reset(void) |
131 | void I2C_Reset(void) |
132 | { |
132 | { |
133 | // stop i2c bus |
133 | // stop i2c bus |
134 | I2C_Stop(); |
134 | I2C_Stop(); |
135 | twi_state = 0; |
135 | twi_state = 0; |
136 | motor_write = TWDR; |
136 | motor_write = TWDR; |
137 | motor_write = 0; |
137 | motor_write = 0; |
138 | motor_read = 0; |
138 | motor_read = 0; |
139 | TWCR = (1<<TWINT); // reset to original state incl. interrupt flag reset |
139 | TWCR = (1<<TWINT); // reset to original state incl. interrupt flag reset |
140 | TWAMR = 0; |
140 | TWAMR = 0; |
141 | TWAR = 0; |
141 | TWAR = 0; |
142 | TWDR = 0; |
142 | TWDR = 0; |
143 | TWSR = 0; |
143 | TWSR = 0; |
144 | TWBR = 0; |
144 | TWBR = 0; |
145 | I2C_Init(); |
145 | I2C_Init(); |
146 | I2C_Start(); |
146 | I2C_Start(); |
147 | I2C_WriteByte(0); |
147 | I2C_WriteByte(0); |
148 | } |
148 | } |
149 | 149 | ||
150 | /****************************************/ |
150 | /****************************************/ |
151 | /* I2C ISR */ |
151 | /* I2C ISR */ |
152 | /****************************************/ |
152 | /****************************************/ |
153 | ISR (TWI_vect) |
153 | ISR (TWI_vect) |
154 | 154 | ||
155 | { |
155 | { |
156 | 156 | ||
157 | switch (twi_state++) // First i2c_start from SendMotorData() |
157 | switch (twi_state++) // First i2c_start from SendMotorData() |
158 | { |
158 | { |
159 | // Master Transmit |
159 | // Master Transmit |
160 | case 0: // Send SLA-W |
160 | case 0: // Send SLA-W |
161 | I2C_WriteByte(0x52 + (motor_write * 2) ); |
161 | I2C_WriteByte(0x52 + (motor_write * 2) ); |
162 | break; |
162 | break; |
163 | case 1: // Send Data to Slave |
163 | case 1: // Send Data to Slave |
164 | #ifdef HEXAKOPTER |
164 | #ifdef HEXAKOPTER |
165 | switch(motor_write) |
165 | switch(motor_write) |
166 | { |
166 | { |
167 | case 0: |
167 | case 0: |
168 | I2C_WriteByte(Motor_FrontLeft); |
168 | I2C_WriteByte(Motor_FrontLeft); |
169 | break; |
169 | break; |
170 | case 1: |
170 | case 1: |
171 | I2C_WriteByte(Motor_RearRight); |
171 | I2C_WriteByte(Motor_RearRight); |
172 | break; |
172 | break; |
173 | case 2: |
173 | case 2: |
174 | I2C_WriteByte(Motor_FrontRight); |
174 | I2C_WriteByte(Motor_FrontRight); |
175 | break; |
175 | break; |
176 | case 3: |
176 | case 3: |
177 | I2C_WriteByte(Motor_RearLeft); |
177 | I2C_WriteByte(Motor_RearLeft); |
178 | break; |
178 | break; |
179 | case 4: |
179 | case 4: |
180 | I2C_WriteByte(Motor_Right); |
180 | I2C_WriteByte(Motor_Right); |
181 | break; |
181 | break; |
182 | case 5: |
182 | case 5: |
183 | I2C_WriteByte(Motor_Left); |
183 | I2C_WriteByte(Motor_Left); |
184 | break; |
184 | break; |
185 | } |
185 | } |
186 | 186 | ||
187 | #else |
187 | #else |
188 | switch(motor_write) |
188 | switch(motor_write) |
189 | { |
189 | { |
190 | case 0: |
190 | case 0: |
191 | I2C_WriteByte(Motor_Front); |
191 | I2C_WriteByte(Motor_Front); |
192 | break; |
192 | break; |
193 | case 1: |
193 | case 1: |
194 | I2C_WriteByte(Motor_Rear); |
194 | I2C_WriteByte(Motor_Rear); |
195 | break; |
195 | break; |
196 | case 2: |
196 | case 2: |
197 | I2C_WriteByte(Motor_Right); |
197 | I2C_WriteByte(Motor_Right); |
198 | break; |
198 | break; |
199 | case 3: |
199 | case 3: |
200 | I2C_WriteByte(Motor_Left); |
200 | I2C_WriteByte(Motor_Left); |
201 | break; |
201 | break; |
202 | } |
202 | } |
203 | #endif |
203 | #endif |
204 | break; |
204 | break; |
205 | case 2: // repeat case 0+1 for all motors |
205 | case 2: // repeat case 0+1 for all motors |
206 | I2C_Stop(); |
206 | I2C_Stop(); |
207 | if (motor_write < (MOTOR_COUNT-1)) |
207 | if (motor_write < (MOTOR_COUNT-1)) |
208 | { |
208 | { |
209 | motor_write++; // jump to next motor |
209 | motor_write++; // jump to next motor |
210 | twi_state = 0; // and repeat from state 0 |
210 | twi_state = 0; // and repeat from state 0 |
211 | } |
211 | } |
212 | else |
212 | else |
213 | { // data to last motor send |
213 | { // data to last motor send |
214 | motor_write = 0; // reset motor write counter |
214 | motor_write = 0; // reset motor write counter |
215 | } |
215 | } |
216 | I2C_Start(); // Repeated start -> switch salve or switch Master Transmit -> Master Receive |
216 | I2C_Start(); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
217 | break; |
217 | break; |
218 | // Master Receive |
218 | // Master Receive |
219 | case 3: // Send SLA-R |
219 | case 3: // Send SLA-R |
220 | I2C_WriteByte(0x53 + (motor_read * 2) ); |
220 | I2C_WriteByte(0x53 + (motor_read * 2) ); |
221 | break; |
221 | break; |
222 | case 4: |
222 | case 4: |
223 | //Transmit 1st byte |
223 | //Transmit 1st byte |
224 | I2C_ReceiveByte(); |
224 | I2C_ReceiveByte(); |
225 | break; |
225 | break; |
226 | case 5: //Read 1st byte and transmit 2nd Byte |
226 | case 5: //Read 1st byte and transmit 2nd Byte |
227 | motor_rx[motor_read] = TWDR; |
227 | motor_rx[motor_read] = TWDR; |
228 | I2C_ReceiveLastByte(); |
228 | I2C_ReceiveLastByte(); |
229 | break; |
229 | break; |
230 | case 6: |
230 | case 6: |
231 | //Read 2nd byte |
231 | //Read 2nd byte |
232 | motor_rx[motor_read + 4] = TWDR; |
232 | motor_rx[motor_read + 4] = TWDR; |
233 | motor_read++; |
233 | motor_read++; |
234 | if (motor_read > (MOTOR_COUNT-1)) motor_read = 0; |
234 | if (motor_read > (MOTOR_COUNT-1)) motor_read = 0; |
235 | I2C_Stop(); |
235 | I2C_Stop(); |
236 | twi_state = 0; |
236 | twi_state = 0; |
237 | I2CTimeout = 10; |
237 | I2CTimeout = 10; |
238 | break; |
238 | break; |
239 | 239 | ||
240 | // Gyro-Offsets |
240 | // Gyro-Offsets |
241 | case 7: |
241 | case 7: |
242 | I2C_WriteByte(0x98); // Address the DAC |
242 | I2C_WriteByte(0x98); // Address the DAC |
243 | break; |
243 | break; |
244 | 244 | ||
245 | case 8: |
245 | case 8: |
246 | I2C_WriteByte(0x10 + (dac_channel * 2)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
246 | I2C_WriteByte(0x10 + (dac_channel * 2)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
247 | break; |
247 | break; |
248 | 248 | ||
249 | case 9: |
249 | case 9: |
250 | switch(dac_channel) |
250 | switch(dac_channel) |
251 | { |
251 | { |
252 | case 0: |
252 | case 0: |
253 | I2C_WriteByte(AnalogOffsetNick); // 1st byte for Channel A |
253 | I2C_WriteByte(AnalogOffsetNick); // 1st byte for Channel A |
254 | break; |
254 | break; |
255 | case 1: |
255 | case 1: |
256 | I2C_WriteByte(AnalogOffsetRoll); // 1st byte for Channel B |
256 | I2C_WriteByte(AnalogOffsetRoll); // 1st byte for Channel B |
257 | break; |
257 | break; |
258 | case 2: |
258 | case 2: |
259 | I2C_WriteByte(AnalogOffsetYaw ); // 1st byte for Channel C |
259 | I2C_WriteByte(AnalogOffsetYaw ); // 1st byte for Channel C |
260 | break; |
260 | break; |
261 | } |
261 | } |
262 | break; |
262 | break; |
263 | 263 | ||
264 | case 10: |
264 | case 10: |
265 | I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
265 | I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
266 | break; |
266 | break; |
267 | 267 | ||
268 | case 11: |
268 | case 11: |
269 | I2C_Stop(); |
269 | I2C_Stop(); |
270 | I2CTimeout = 10; |
270 | I2CTimeout = 10; |
271 | // repeat case 7...10 until all DAC Channels are updated |
271 | // repeat case 7...10 until all DAC Channels are updated |
272 | if(dac_channel < 2) |
272 | if(dac_channel < 2) |
273 | { |
273 | { |
274 | dac_channel ++; // jump to next channel |
274 | dac_channel ++; // jump to next channel |
275 | twi_state = 7; // and repeat from state 7 |
275 | twi_state = 7; // and repeat from state 7 |
276 | I2C_Start(); // start transmission for next channel |
276 | I2C_Start(); // start transmission for next channel |
277 | } |
277 | } |
278 | else |
278 | else |
279 | { // data to last motor send |
279 | { // data to last motor send |
280 | dac_channel = 0; // reset dac channel counter |
280 | dac_channel = 0; // reset dac channel counter |
281 | twi_state = 0; // reset twi_state |
281 | twi_state = 0; // reset twi_state |
282 | } |
282 | } |
283 | break; |
283 | break; |
284 | 284 | ||
285 | default: |
285 | default: |
286 | I2C_Stop(); |
286 | I2C_Stop(); |
287 | twi_state = 0; |
287 | twi_state = 0; |
288 | I2CTimeout = 10; |
288 | I2CTimeout = 10; |
289 | motor_write = 0; |
289 | motor_write = 0; |
290 | motor_read = 0; |
290 | motor_read = 0; |
291 | } |
291 | } |
292 | } |
292 | } |
293 | 293 |