Rev 2158 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2158 | Rev 2189 | ||
---|---|---|---|
1 | #include <avr/io.h> |
1 | #include <avr/io.h> |
2 | #include <avr/interrupt.h> |
2 | #include <avr/interrupt.h> |
3 | #include <util/twi.h> |
3 | #include <util/twi.h> |
4 | #include <util/delay.h> |
4 | #include <util/delay.h> |
- | 5 | #include <stdio.h> |
|
5 | #include "twimaster.h" |
6 | #include "twimaster.h" |
6 | #include "configuration.h" |
7 | //#include "configuration.h" |
- | 8 | //#include "controlMixer.h" |
|
7 | #include "analog.h" |
9 | #include "analog.h" |
8 | #include "printf_P.h" |
- | |
9 | 10 | ||
10 | volatile uint8_t twi_state = TWI_STATE_MOTOR_TX; |
11 | volatile uint8_t twi_state; |
11 | volatile uint8_t dac_channel = 0; |
12 | volatile uint8_t dac_channel; |
12 | volatile uint8_t motor_write = 0; |
13 | volatile uint8_t writeIndex; |
13 | volatile uint8_t motor_read = 0; |
14 | volatile uint8_t readIndex; |
14 | volatile uint16_t I2CTimeout = 100; |
15 | volatile uint16_t I2CTimeout = 100; |
15 | uint8_t missingMotor = 0; |
- | |
16 | 16 | uint8_t missingMotor; |
|
17 | motorData_t motor[MAX_MOTORS]; |
- | |
18 | 17 | MLBLC_t mkblcs[MAX_I2CCHANNELS]; |
|
19 | uint8_t DACChannel = 0; |
18 | uint8_t DACChannel; |
20 | 19 | ||
21 | #define SCL_CLOCK 200000L |
20 | #define SCL_CLOCK 200000L |
22 | #define I2C_TIMEOUT 30000 |
21 | #define I2C_TIMEOUT 30000 |
23 | 22 | ||
24 | /************************************************** |
23 | /************************************************** |
25 | * Initialize I2C (TWI) |
24 | * Initialize I2C (TWI) |
26 | **************************************************/ |
25 | **************************************************/ |
27 | void I2C_init(void) { |
26 | void I2C_init(void) { |
28 | uint8_t i; |
27 | uint8_t i; |
29 | uint8_t sreg = SREG; |
28 | uint8_t sreg = SREG; |
30 | cli(); |
29 | cli(); |
31 | 30 | ||
32 | // SDA is INPUT |
31 | // SDA is INPUT |
33 | DDRC &= ~(1 << DDC1); |
32 | DDRC &= ~(1 << DDC1); |
34 | // SCL is output |
33 | // SCL is output |
35 | DDRC |= (1 << DDC0); |
34 | DDRC |= (1 << DDC0); |
36 | // pull up SDA |
35 | // pull up SDA |
37 | PORTC |= (1 << PORTC0) | (1 << PORTC1); |
36 | PORTC |= (1 << PORTC0) | (1 << PORTC1); |
38 | 37 | ||
39 | // TWI Status Register |
38 | // TWI Status Register |
40 | // prescaler 1 (TWPS1 = 0, TWPS0 = 0) |
39 | // prescaler 1 (TWPS1 = 0, TWPS0 = 0) |
41 | TWSR &= ~((1 << TWPS1) | (1 << TWPS0)); |
40 | TWSR &= ~((1 << TWPS1) | (1 << TWPS0)); |
42 | 41 | ||
43 | // set TWI Bit Rate Register |
42 | // set TWI Bit Rate Register |
44 | TWBR = ((SYSCLK / SCL_CLOCK) - 16) / 2; |
43 | TWBR = ((F_CPU / SCL_CLOCK) - 16) / 2; |
45 | 44 | ||
46 | twi_state = TWI_STATE_MOTOR_TX; |
45 | twi_state = TWI_STATE_MOTOR_TX; |
47 | motor_write = 0; |
46 | writeIndex = 0; |
48 | motor_read = 0; |
47 | readIndex = 0; |
49 | - | ||
50 | for (i = 0; i < MAX_MOTORS; i++) { |
48 | |
51 | motor[i].throttle = 0; |
49 | for (i = 0; i < MAX_I2CCHANNELS; i++) { |
52 | motor[i].present = 0; |
50 | mkblcs[i].present = 0; |
53 | motor[i].maxPWM = 0; |
51 | mkblcs[i].maxPWM = 0; |
54 | } |
52 | } |
55 | 53 | ||
56 | SREG = sreg; |
54 | SREG = sreg; |
57 | } |
55 | } |
58 | 56 | ||
59 | /**************************************** |
57 | /**************************************** |
60 | * Start I2C |
58 | * Start I2C |
61 | ****************************************/ |
59 | ****************************************/ |
62 | void I2C_Start(uint8_t start_state) { |
60 | void I2C_start(uint8_t start_state) { |
63 | twi_state = start_state; |
61 | twi_state = start_state; |
64 | // TWI Control Register |
62 | // TWI Control Register |
65 | // clear TWI interrupt flag (TWINT=1) |
63 | // clear TWI interrupt flag (TWINT=1) |
66 | // disable TWI Acknowledge Bit (TWEA = 0) |
64 | // disable TWI Acknowledge Bit (TWEA = 0) |
67 | // enable TWI START Condition Bit (TWSTA = 1), MASTER |
65 | // enable TWI START Condition Bit (TWSTA = 1), MASTER |
68 | // disable TWI STOP Condition Bit (TWSTO = 0) |
66 | // disable TWI STOP Condition Bit (TWSTO = 0) |
69 | // disable TWI Write Collision Flag (TWWC = 0) |
67 | // disable TWI Write Collision Flag (TWWC = 0) |
70 | // enable i2c (TWEN = 1) |
68 | // enable i2c (TWEN = 1) |
71 | // enable TWI Interrupt (TWIE = 1) |
69 | // enable TWI Interrupt (TWIE = 1) |
72 | TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN) | (1 << TWIE); |
70 | TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN) | (1 << TWIE); |
73 | } |
71 | } |
74 | 72 | ||
75 | /**************************************** |
73 | /**************************************** |
76 | * Stop I2C |
74 | * Stop I2C |
77 | ****************************************/ |
75 | ****************************************/ |
78 | void I2C_Stop(uint8_t start_state) { |
76 | void I2C_stop(uint8_t start_state) { |
79 | twi_state = start_state; |
77 | twi_state = start_state; |
80 | // TWI Control Register |
78 | // TWI Control Register |
81 | // clear TWI interrupt flag (TWINT=1) |
79 | // clear TWI interrupt flag (TWINT=1) |
82 | // disable TWI Acknowledge Bit (TWEA = 0) |
80 | // disable TWI Acknowledge Bit (TWEA = 0) |
83 | // diable TWI START Condition Bit (TWSTA = 1), no MASTER |
81 | // diable TWI START Condition Bit (TWSTA = 1), no MASTER |
84 | // enable TWI STOP Condition Bit (TWSTO = 1) |
82 | // enable TWI STOP Condition Bit (TWSTO = 1) |
85 | // disable TWI Write Collision Flag (TWWC = 0) |
83 | // disable TWI Write Collision Flag (TWWC = 0) |
86 | // enable i2c (TWEN = 1) |
84 | // enable i2c (TWEN = 1) |
87 | // disable TWI Interrupt (TWIE = 0) |
85 | // disable TWI Interrupt (TWIE = 0) |
88 | TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN); |
86 | TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN); |
89 | } |
87 | } |
90 | 88 | ||
91 | /**************************************** |
89 | /**************************************** |
92 | * Write to I2C |
90 | * Write to I2C |
93 | ****************************************/ |
91 | ****************************************/ |
94 | void I2C_WriteByte(int8_t byte) { |
92 | void I2C_writeByte(int8_t byte) { |
95 | // move byte to send into TWI Data Register |
93 | // move byte to send into TWI Data Register |
96 | TWDR = byte; |
94 | TWDR = byte; |
97 | // clear interrupt flag (TWINT = 1) |
95 | // clear interrupt flag (TWINT = 1) |
98 | // enable i2c bus (TWEN = 1) |
96 | // enable i2c bus (TWEN = 1) |
99 | // enable interrupt (TWIE = 1) |
97 | // enable interrupt (TWIE = 1) |
100 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE); |
98 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE); |
101 | } |
99 | } |
102 | 100 | ||
103 | /**************************************** |
101 | /**************************************** |
104 | * Receive byte and send ACK |
102 | * Receive byte and send ACK |
105 | ****************************************/ |
103 | ****************************************/ |
106 | void I2C_ReceiveByte(void) { |
104 | void I2C_receiveByte(void) { |
107 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE) | (1 << TWEA); |
105 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE) | (1 << TWEA); |
108 | } |
106 | } |
109 | 107 | ||
110 | /**************************************** |
108 | /**************************************** |
111 | * I2C receive last byte and send no ACK |
109 | * I2C receive last byte and send no ACK |
112 | ****************************************/ |
110 | ****************************************/ |
113 | void I2C_ReceiveLastByte(void) { |
111 | void I2C_receiveLastByte(void) { |
114 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE); |
112 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE); |
115 | } |
113 | } |
116 | 114 | ||
117 | /**************************************** |
115 | /**************************************** |
118 | * Reset I2C |
116 | * Reset I2C |
119 | ****************************************/ |
117 | ****************************************/ |
120 | void I2C_Reset(void) { |
118 | void I2C_reset(void) { |
121 | // stop i2c bus |
119 | // stop i2c bus |
122 | I2C_Stop(TWI_STATE_MOTOR_TX); |
120 | I2C_stop(TWI_STATE_MOTOR_TX); |
123 | twi_state = 0; |
121 | twi_state = 0; |
124 | motor_write = TWDR; |
122 | writeIndex = TWDR; |
125 | motor_write = 0; |
123 | writeIndex = 0; |
126 | motor_read = 0; |
124 | readIndex = 0; |
127 | TWCR = (1 << TWINT); // reset to original state incl. interrupt flag reset |
125 | TWCR = (1 << TWINT); // reset to original state incl. interrupt flag reset |
128 | TWAMR = 0; |
126 | TWAMR = 0; |
129 | TWAR = 0; |
127 | TWAR = 0; |
130 | TWDR = 0; |
128 | TWDR = 0; |
131 | TWSR = 0; |
129 | TWSR = 0; |
132 | TWBR = 0; |
130 | TWBR = 0; |
133 | I2C_init(); |
131 | I2C_init(); |
134 | I2C_Start(TWI_STATE_MOTOR_TX); |
132 | I2C_start(TWI_STATE_MOTOR_TX); |
135 | } |
133 | } |
136 | 134 | ||
137 | /**************************************** |
135 | /**************************************** |
138 | * I2C ISR |
136 | * I2C ISR |
139 | ****************************************/ |
137 | ****************************************/ |
140 | ISR (TWI_vect) |
138 | ISR (TWI_vect) |
141 | { |
139 | { |
142 | static uint8_t missing_motor = 0; |
140 | static uint8_t missing_motor = 0; |
143 | switch (twi_state++) { // First i2c_start from SendMotorData() |
141 | switch (twi_state++) { // First i2c_start from SendMotorData() |
144 | // Master Transmit |
142 | // Master Transmit |
145 | case 0: // TWI_STATE_MOTOR_TX |
143 | case 0: // TWI_STATE_MOTOR_TX |
146 | // skip motor if not used in mixer |
144 | // skip motor if not used in mixer |
147 | while ((motorMixer.matrix[motor_write][MIX_THROTTLE] <= 0) && (motor_write < MAX_MOTORS)) |
145 | while ((outputMixer[writeIndex].outputType != OUTPUT_TYPE_MOTOR) && (writeIndex < MAX_I2CCHANNELS)) |
148 | motor_write++; |
146 | writeIndex++; |
149 | if (motor_write >= MAX_MOTORS) { // writing finished, read now |
147 | if (writeIndex >= MAX_I2CCHANNELS) { // writing finished, read now |
150 | motor_write = 0; |
148 | writeIndex = 0; |
151 | twi_state = TWI_STATE_MOTOR_RX; |
149 | twi_state = TWI_STATE_MOTOR_RX; |
152 | I2C_WriteByte(0x53 + (motor_read * 2)); // select slave adress in rx mode |
150 | I2C_writeByte(0x53 + (readIndex * 2)); // select slave adress in rx mode |
153 | } else |
151 | } else |
154 | I2C_WriteByte(0x52 + (motor_write * 2)); // select slave adress in tx mode |
152 | I2C_writeByte(0x52 + (writeIndex * 2)); // select slave adress in tx mode |
155 | break; |
153 | break; |
156 | case 1: // Send Data to Slave |
154 | case 1: // Send Data to Slave |
- | 155 | //I2C_writeByte(outputs[writeIndex]>>LOG_CONTROL_OUTPUT_SCALING); // transmit throttle value. |
|
157 | I2C_WriteByte(motor[motor_write].throttle); // transmit throttle value. |
156 | I2C_writeByte(mkblcs[writeIndex].throttle); // transmit throttle value. |
158 | break; |
157 | break; |
159 | case 2: // repeat case 0+1 for all motors |
158 | case 2: // repeat case 0+1 for all motors |
160 | if (TWSR == TW_MT_DATA_NACK) { // Data transmitted, NACK received |
159 | if (TWSR == TW_MT_DATA_NACK) { // Data transmitted, NACK received |
161 | if (!missing_motor) |
160 | if (!missing_motor) |
162 | missing_motor = motor_write + 1; |
161 | missing_motor = writeIndex + 1; |
163 | if (++motor[motor_write].error == 0) |
162 | if (++mkblcs[writeIndex].error == 0) |
164 | motor[motor_write].error = 255; // increment error counter and handle overflow |
163 | mkblcs[writeIndex].error = 255; // increment error counter and handle overflow |
165 | } |
164 | } |
166 | I2C_Stop(TWI_STATE_MOTOR_TX); |
165 | I2C_stop(TWI_STATE_MOTOR_TX); |
167 | I2CTimeout = 10; |
166 | I2CTimeout = 10; |
168 | motor_write++; // next motor |
167 | writeIndex++; // next motor |
169 | I2C_Start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
168 | I2C_start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
170 | break; |
169 | break; |
171 | // Master Receive Data |
170 | // Master Receive Data |
172 | case 3: |
171 | case 3: |
173 | if (TWSR != TW_MR_SLA_ACK) { // SLA+R transmitted, if not ACK received |
172 | if (TWSR != TW_MR_SLA_ACK) { // SLA+R transmitted, if not ACK received |
174 | // no response from the addressed slave received |
173 | // no response from the addressed slave received |
175 | motor[motor_read].present = 0; |
174 | mkblcs[readIndex].present = 0; |
176 | motor_read++; // next motor |
175 | readIndex++; // next motor |
177 | if (motor_read >= MAX_MOTORS) |
176 | if (readIndex >= MAX_I2CCHANNELS) |
178 | motor_read = 0; // restart reading of first motor if we have reached the last one |
177 | readIndex = 0; // restart reading of first motor if we have reached the last one |
179 | I2C_Stop(TWI_STATE_MOTOR_TX); |
178 | I2C_stop(TWI_STATE_MOTOR_TX); |
180 | } else { |
179 | } else { |
181 | motor[motor_read].present = ('1' - '-') + motor_read; |
180 | mkblcs[readIndex].present = ('1' - '-') + readIndex; |
182 | I2C_ReceiveByte(); //Transmit 1st byte |
181 | I2C_receiveByte(); //Transmit 1st byte |
183 | } |
182 | } |
184 | missingMotor = missing_motor; |
183 | missingMotor = missing_motor; |
185 | missing_motor = 0; |
184 | missing_motor = 0; |
186 | break; |
185 | break; |
187 | case 4: //Read 1st byte and transmit 2nd Byte |
186 | case 4: //Read 1st byte and transmit 2nd Byte |
188 | motor[motor_read].current = TWDR; |
187 | mkblcs[readIndex].current = TWDR; |
189 | I2C_ReceiveLastByte(); // nack |
188 | I2C_receiveLastByte(); // nack |
190 | break; |
189 | break; |
191 | case 5: |
190 | case 5: |
192 | //Read 2nd byte |
191 | //Read 2nd byte |
193 | motor[motor_read].maxPWM = TWDR; |
192 | mkblcs[readIndex].maxPWM = TWDR; |
194 | motor_read++; // next motor |
193 | readIndex++; // next motor |
195 | if (motor_read >= MAX_MOTORS) |
194 | if (readIndex >= MAX_I2CCHANNELS) |
196 | motor_read = 0; // restart reading of first motor if we have reached the last one |
195 | readIndex = 0; // restart reading of first motor if we have reached the last one |
197 | I2C_Stop(TWI_STATE_MOTOR_TX); |
196 | I2C_stop(TWI_STATE_MOTOR_TX); |
198 | break; |
197 | break; |
199 | 198 | ||
200 | // Writing ADC values. |
199 | // Writing ADC values. |
201 | case 7: |
200 | case 7: |
202 | I2C_WriteByte(0x98); // Address the DAC |
201 | I2C_writeByte(0x98); // Address the DAC |
203 | break; |
202 | break; |
204 | 203 | ||
205 | case 8: |
204 | case 8: |
206 | I2C_WriteByte(0x10 + (DACChannel << 1)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
205 | I2C_writeByte(0x10 + (DACChannel << 1)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
207 | break; |
206 | break; |
208 | 207 | ||
209 | case 9: |
208 | case 9: |
210 | I2C_WriteByte(gyroAmplifierOffset.offsets[DACChannel]); |
209 | I2C_writeByte(gyroAmplifierOffset.offsets[DACChannel]); |
211 | break; |
210 | break; |
212 | 211 | ||
213 | case 10: |
212 | case 10: |
214 | I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
213 | I2C_writeByte(0x80); // 2nd byte for all channels is 0x80 |
215 | break; |
214 | break; |
216 | 215 | ||
217 | case 11: |
216 | case 11: |
218 | I2C_Stop(TWI_STATE_MOTOR_TX); |
217 | I2C_stop(TWI_STATE_MOTOR_TX); |
219 | I2CTimeout = 10; |
218 | I2CTimeout = 10; |
220 | // repeat case 7...10 until all DAC Channels are updated |
219 | // repeat case 7...10 until all DAC Channels are updated |
221 | if (DACChannel < 2) { |
220 | if (DACChannel < 2) { |
222 | DACChannel++; // jump to next channel |
221 | DACChannel++; // jump to next channel |
223 | I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel |
222 | I2C_start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel |
224 | } else { |
223 | } else { |
225 | DACChannel = 0; // reset dac channel counter |
224 | DACChannel = 0; // reset dac channel counter |
226 | } |
225 | } |
227 | break; |
226 | break; |
228 | 227 | ||
229 | default: |
228 | default: |
230 | I2C_Stop(TWI_STATE_MOTOR_TX); |
229 | I2C_stop(TWI_STATE_MOTOR_TX); |
231 | I2CTimeout = 10; |
230 | I2CTimeout = 10; |
232 | motor_write = 0; |
231 | writeIndex = 0; |
233 | motor_read = 0; |
232 | readIndex = 0; |
234 | } |
233 | } |
235 | } |
234 | } |
236 | 235 | ||
237 | extern void twi_diagnostics(void) { |
236 | extern void twi_diagnostics(void) { |
238 | // Check connected BL-Ctrls |
237 | // Check connected BL-Ctrls |
239 | uint8_t i; |
238 | uint8_t i; |
240 | 239 | ||
241 | printf("\n\rFound BL-Ctrl: "); |
240 | printf("\n\rFound BL-Ctrl: "); |
242 | 241 | ||
243 | for (i = 0; i < MAX_MOTORS; i++) { |
242 | for (i=0; i<MAX_I2CCHANNELS; i++) { |
244 | motor[i].throttle = 0; |
243 | mkblcs[i].throttle = 0; |
245 | } |
244 | } |
246 | 245 | ||
247 | I2C_Start(TWI_STATE_MOTOR_TX); |
246 | I2C_start(TWI_STATE_MOTOR_TX); |
248 | _delay_ms(2); |
247 | _delay_ms(2); |
249 | 248 | ||
250 | motor_read = 0; // read the first I2C-Data |
249 | readIndex = 0; // read the first I2C-Data |
251 | 250 | ||
252 | for (i = 0; i < MAX_MOTORS; i++) { |
251 | for (i = 0; i < MAX_I2CCHANNELS; i++) { |
253 | I2C_Start(TWI_STATE_MOTOR_TX); |
252 | I2C_start(TWI_STATE_MOTOR_TX); |
254 | _delay_ms(2); |
253 | _delay_ms(2); |
255 | if (motor[i].present) |
254 | if (mkblcs[i].present) |
256 | printf("%d ",i+1); |
255 | printf("%d ",i+1); |
257 | } |
256 | } |
258 | 257 | ||
259 | for (i = 0; i < MAX_MOTORS; i++) { |
258 | for (i = 0; i < MAX_I2CCHANNELS; i++) { |
260 | if (!motor[i].present && motorMixer.matrix[i][MIX_THROTTLE] > 0) |
259 | if (!mkblcs[i].present && outputMixer[i].outputType == OUTPUT_TYPE_MOTOR) |
261 | printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i + 1); |
260 | printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i + 1); |
262 | motor[i].error = 0; |
261 | mkblcs[i].error = 0; |
263 | } |
262 | } |
264 | } |
263 | } |
265 | 264 |