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