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