Rev 936 | Go to most recent revision | Only display areas with differences | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 936 | Rev 952 | ||
---|---|---|---|
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[8]; |
16 | volatile uint8_t motor_rx[8]; |
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 | switch(motor_write) |
164 | switch(motor_write) |
165 | { |
165 | { |
166 | case 0: |
166 | case 0: |
167 | I2C_WriteByte(Motor_Front); |
167 | I2C_WriteByte(Motor_Front); |
168 | break; |
168 | break; |
169 | case 1: |
169 | case 1: |
170 | I2C_WriteByte(Motor_Rear); |
170 | I2C_WriteByte(Motor_Rear); |
171 | break; |
171 | break; |
172 | case 2: |
172 | case 2: |
173 | I2C_WriteByte(Motor_Right); |
173 | I2C_WriteByte(Motor_Right); |
174 | break; |
174 | break; |
175 | case 3: |
175 | case 3: |
176 | I2C_WriteByte(Motor_Left); |
176 | I2C_WriteByte(Motor_Left); |
177 | break; |
177 | break; |
178 | } |
178 | } |
179 | break; |
179 | break; |
180 | case 2: // repeat case 0+1 for all motors |
180 | case 2: // repeat case 0+1 for all motors |
181 | I2C_Stop(); |
181 | I2C_Stop(); |
182 | if (motor_write < 3) |
182 | if (motor_write < 3) |
183 | { |
183 | { |
184 | motor_write++; // jump to next motor |
184 | motor_write++; // jump to next motor |
185 | twi_state = 0; // and repeat from state 0 |
185 | twi_state = 0; // and repeat from state 0 |
186 | } |
186 | } |
187 | else |
187 | else |
188 | { // data to last motor send |
188 | { // data to last motor send |
189 | motor_write = 0; // reset motor write counter |
189 | motor_write = 0; // reset motor write counter |
190 | } |
190 | } |
191 | I2C_Start(); // Repeated start -> switch salve or switch Master Transmit -> Master Receive |
191 | I2C_Start(); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
192 | break; |
192 | break; |
193 | 193 | ||
194 | // Master Receive |
194 | // Master Receive |
195 | case 3: // Send SLA-R |
195 | case 3: // Send SLA-R |
196 | I2C_WriteByte(0x53 + (motor_read * 2) ); |
196 | I2C_WriteByte(0x53 + (motor_read * 2) ); |
197 | break; |
197 | break; |
198 | case 4: |
198 | case 4: |
199 | //Transmit 1st byte |
199 | //Transmit 1st byte |
200 | I2C_ReceiveByte(); |
200 | I2C_ReceiveByte(); |
201 | break; |
201 | break; |
202 | case 5: //Read 1st byte and transmit 2nd Byte |
202 | case 5: //Read 1st byte and transmit 2nd Byte |
203 | motor_rx[motor_read] = TWDR; |
203 | motor_rx[motor_read] = TWDR; |
204 | I2C_ReceiveLastByte(); |
204 | I2C_ReceiveLastByte(); |
205 | break; |
205 | break; |
206 | case 6: |
206 | case 6: |
207 | //Read 2nd byte |
207 | //Read 2nd byte |
208 | motor_rx[motor_read + 4] = TWDR; |
208 | motor_rx[motor_read + 4] = TWDR; |
209 | motor_read++; |
209 | motor_read++; |
210 | if (motor_read > 3) motor_read = 0; |
210 | if (motor_read > 3) motor_read = 0; |
211 | I2C_Stop(); |
211 | I2C_Stop(); |
212 | twi_state = 0; |
212 | twi_state = 0; |
213 | I2CTimeout = 10; |
213 | I2CTimeout = 10; |
214 | break; |
214 | break; |
215 | 215 | ||
216 | // Gyro-Offsets |
216 | // Gyro-Offsets |
217 | case 7: |
217 | case 7: |
218 | I2C_WriteByte(0x98); // Address the DAC |
218 | I2C_WriteByte(0x98); // Address the DAC |
219 | break; |
219 | break; |
220 | 220 | ||
221 | case 8: |
221 | case 8: |
222 | I2C_WriteByte(0x10 + (dac_channel * 2)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
222 | I2C_WriteByte(0x10 + (dac_channel * 2)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
223 | break; |
223 | break; |
224 | 224 | ||
225 | case 9: |
225 | case 9: |
226 | switch(dac_channel) |
226 | switch(dac_channel) |
227 | { |
227 | { |
228 | case 0: |
228 | case 0: |
229 | I2C_WriteByte(AnalogOffsetNick); // 1st byte for Channel A |
229 | I2C_WriteByte(AnalogOffsetNick); // 1st byte for Channel A |
230 | break; |
230 | break; |
231 | case 1: |
231 | case 1: |
232 | I2C_WriteByte(AnalogOffsetRoll); // 1st byte for Channel B |
232 | I2C_WriteByte(AnalogOffsetRoll); // 1st byte for Channel B |
233 | break; |
233 | break; |
234 | case 2: |
234 | case 2: |
235 | I2C_WriteByte(AnalogOffsetYaw ); // 1st byte for Channel C |
235 | I2C_WriteByte(AnalogOffsetYaw ); // 1st byte for Channel C |
236 | break; |
236 | break; |
237 | } |
237 | } |
238 | break; |
238 | break; |
239 | 239 | ||
240 | case 10: |
240 | case 10: |
241 | I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
241 | I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
242 | break; |
242 | break; |
243 | 243 | ||
244 | case 11: |
244 | case 11: |
245 | I2C_Stop(); |
245 | I2C_Stop(); |
246 | I2CTimeout = 10; |
246 | I2CTimeout = 10; |
247 | // repeat case 7...10 until all DAC Channels are updated |
247 | // repeat case 7...10 until all DAC Channels are updated |
248 | if(dac_channel < 2) |
248 | if(dac_channel < 2) |
249 | { |
249 | { |
250 | dac_channel ++; // jump to next channel |
250 | dac_channel ++; // jump to next channel |
251 | twi_state = 7; // and repeat from state 7 |
251 | twi_state = 7; // and repeat from state 7 |
252 | I2C_Start(); // start transmission for next channel |
252 | I2C_Start(); // start transmission for next channel |
253 | } |
253 | } |
254 | else |
254 | else |
255 | { // data to last motor send |
255 | { // data to last motor send |
256 | dac_channel = 0; // reset dac channel counter |
256 | dac_channel = 0; // reset dac channel counter |
257 | twi_state = 0; // reset twi_state |
257 | twi_state = 0; // reset twi_state |
258 | } |
258 | } |
259 | break; |
259 | break; |
260 | 260 | ||
261 | default: |
261 | default: |
262 | I2C_Stop(); |
262 | I2C_Stop(); |
263 | twi_state = 0; |
263 | twi_state = 0; |
264 | I2CTimeout = 10; |
264 | I2CTimeout = 10; |
265 | motor_write = 0; |
265 | motor_write = 0; |
266 | motor_read = 0; |
266 | motor_read = 0; |
267 | } |
267 | } |
268 | } |
268 | } |
269 | 269 |