Rev 1775 | Rev 1868 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1775 | Rev 1821 | ||
---|---|---|---|
Line 57... | Line 57... | ||
57 | #include "twimaster.h" |
57 | #include "twimaster.h" |
58 | //#include "analog.h" |
58 | //#include "analog.h" |
59 | #include "configuration.h" |
59 | #include "configuration.h" |
60 | #include "printf_P.h" |
60 | #include "printf_P.h" |
Line 61... | Line 61... | ||
61 | 61 | ||
62 | volatile uint8_t twi_state = TWI_STATE_MOTOR_TX; |
62 | volatile uint8_t twi_state = TWI_STATE_MOTOR_TX; |
63 | volatile uint8_t dac_channel = 0; |
63 | volatile uint8_t dac_channel = 0; |
64 | volatile uint8_t motor_write = 0; |
64 | volatile uint8_t motor_write = 0; |
65 | volatile uint8_t motor_read = 0; |
65 | volatile uint8_t motor_read = 0; |
66 | volatile uint16_t I2CTimeout = 100; |
66 | volatile uint16_t I2CTimeout = 100; |
Line 67... | Line 67... | ||
67 | uint8_t missingMotor = 0; |
67 | uint8_t missingMotor = 0; |
Line 68... | Line 68... | ||
68 | 68 | ||
69 | MotorData_t motor[MAX_MOTORS]; |
69 | MotorData_t motor[MAX_MOTORS]; |
Line 76... | Line 76... | ||
76 | 76 | ||
77 | /************************************************** |
77 | /************************************************** |
78 | * Initialize I2C (TWI) |
78 | * Initialize I2C (TWI) |
79 | **************************************************/ |
79 | **************************************************/ |
80 | void I2C_init(void) { |
80 | void I2C_init(void) { |
81 | uint8_t i; |
81 | uint8_t i; |
82 | uint8_t sreg = SREG; |
82 | uint8_t sreg = SREG; |
83 | cli(); |
83 | cli(); |
84 | 84 | ||
85 | // SDA is INPUT |
85 | // SDA is INPUT |
86 | DDRC &= ~(1<<DDC1); |
86 | DDRC &= ~(1 << DDC1); |
87 | // SCL is output |
87 | // SCL is output |
88 | DDRC |= (1<<DDC0); |
88 | DDRC |= (1 << DDC0); |
89 | // pull up SDA |
89 | // pull up SDA |
90 | PORTC |= (1<<PORTC0)|(1<<PORTC1); |
90 | PORTC |= (1 << PORTC0) | (1 << PORTC1); |
91 | 91 | ||
92 | // TWI Status Register |
92 | // TWI Status Register |
93 | // prescaler 1 (TWPS1 = 0, TWPS0 = 0) |
93 | // prescaler 1 (TWPS1 = 0, TWPS0 = 0) |
94 | TWSR &= ~((1<<TWPS1)|(1<<TWPS0)); |
94 | TWSR &= ~((1 << TWPS1) | (1 << TWPS0)); |
95 | 95 | ||
96 | // set TWI Bit Rate Register |
96 | // set TWI Bit Rate Register |
97 | TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
97 | TWBR = ((SYSCLK / SCL_CLOCK) - 16) / 2; |
98 | 98 | ||
99 | twi_state = TWI_STATE_MOTOR_TX; |
99 | twi_state = TWI_STATE_MOTOR_TX; |
100 | motor_write = 0; |
100 | motor_write = 0; |
101 | motor_read = 0; |
101 | motor_read = 0; |
102 | 102 | ||
103 | for(i=0; i < MAX_MOTORS; i++) { |
103 | for (i = 0; i < MAX_MOTORS; i++) { |
104 | motor[i].SetPoint = 0; |
104 | motor[i].SetPoint = 0; |
105 | motor[i].Present = 0; |
105 | motor[i].Present = 0; |
106 | motor[i].Error = 0; |
106 | motor[i].Error = 0; |
107 | motor[i].MaxPWM = 0; |
107 | motor[i].MaxPWM = 0; |
108 | } |
108 | } |
109 | 109 | ||
110 | SREG = sreg; |
110 | SREG = sreg; |
Line 111... | Line 111... | ||
111 | } |
111 | } |
112 | 112 | ||
113 | /**************************************** |
113 | /**************************************** |
114 | * Start I2C |
114 | * Start I2C |
115 | ****************************************/ |
115 | ****************************************/ |
116 | void I2C_Start(uint8_t start_state) { |
116 | void I2C_Start(uint8_t start_state) { |
117 | twi_state = start_state; |
117 | twi_state = start_state; |
118 | // TWI Control Register |
118 | // TWI Control Register |
119 | // clear TWI interrupt flag (TWINT=1) |
119 | // clear TWI interrupt flag (TWINT=1) |
120 | // disable TWI Acknowledge Bit (TWEA = 0) |
120 | // disable TWI Acknowledge Bit (TWEA = 0) |
121 | // enable TWI START Condition Bit (TWSTA = 1), MASTER |
121 | // enable TWI START Condition Bit (TWSTA = 1), MASTER |
122 | // disable TWI STOP Condition Bit (TWSTO = 0) |
122 | // disable TWI STOP Condition Bit (TWSTO = 0) |
123 | // disable TWI Write Collision Flag (TWWC = 0) |
123 | // disable TWI Write Collision Flag (TWWC = 0) |
124 | // enable i2c (TWEN = 1) |
124 | // enable i2c (TWEN = 1) |
125 | // enable TWI Interrupt (TWIE = 1) |
125 | // enable TWI Interrupt (TWIE = 1) |
Line 126... | Line 126... | ||
126 | TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWIE); |
126 | TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN) | (1 << TWIE); |
127 | } |
127 | } |
128 | 128 | ||
129 | /**************************************** |
129 | /**************************************** |
130 | * Stop I2C |
130 | * Stop I2C |
131 | ****************************************/ |
131 | ****************************************/ |
132 | void I2C_Stop(uint8_t start_state) { |
132 | void I2C_Stop(uint8_t start_state) { |
133 | twi_state = start_state; |
133 | twi_state = start_state; |
134 | // TWI Control Register |
134 | // TWI Control Register |
135 | // clear TWI interrupt flag (TWINT=1) |
135 | // clear TWI interrupt flag (TWINT=1) |
136 | // disable TWI Acknowledge Bit (TWEA = 0) |
136 | // disable TWI Acknowledge Bit (TWEA = 0) |
137 | // diable TWI START Condition Bit (TWSTA = 1), no MASTER |
137 | // diable TWI START Condition Bit (TWSTA = 1), no MASTER |
138 | // enable TWI STOP Condition Bit (TWSTO = 1) |
138 | // enable TWI STOP Condition Bit (TWSTO = 1) |
139 | // disable TWI Write Collision Flag (TWWC = 0) |
139 | // disable TWI Write Collision Flag (TWWC = 0) |
140 | // enable i2c (TWEN = 1) |
140 | // enable i2c (TWEN = 1) |
Line 141... | Line 141... | ||
141 | // disable TWI Interrupt (TWIE = 0) |
141 | // disable TWI Interrupt (TWIE = 0) |
142 | TWCR = (1<<TWINT) | (1<<TWSTO) | (1<<TWEN); |
142 | TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN); |
143 | } |
143 | } |
144 | 144 | ||
145 | /**************************************** |
145 | /**************************************** |
146 | * Write to I2C |
146 | * Write to I2C |
147 | ****************************************/ |
147 | ****************************************/ |
148 | void I2C_WriteByte(int8_t byte) { |
148 | void I2C_WriteByte(int8_t byte) { |
149 | // move byte to send into TWI Data Register |
149 | // move byte to send into TWI Data Register |
150 | TWDR = byte; |
150 | TWDR = byte; |
151 | // clear interrupt flag (TWINT = 1) |
151 | // clear interrupt flag (TWINT = 1) |
Line 152... | Line 152... | ||
152 | // enable i2c bus (TWEN = 1) |
152 | // enable i2c bus (TWEN = 1) |
153 | // enable interrupt (TWIE = 1) |
153 | // enable interrupt (TWIE = 1) |
154 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
154 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE); |
155 | } |
155 | } |
156 | 156 | ||
157 | /**************************************** |
157 | /**************************************** |
Line 158... | Line 158... | ||
158 | * Receive byte and send ACK |
158 | * Receive byte and send ACK |
159 | ****************************************/ |
159 | ****************************************/ |
160 | void I2C_ReceiveByte(void) { |
160 | void I2C_ReceiveByte(void) { |
161 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA); |
161 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE) | (1 << TWEA); |
162 | } |
162 | } |
163 | 163 | ||
Line 164... | Line 164... | ||
164 | /**************************************** |
164 | /**************************************** |
165 | * I2C receive last byte and send no ACK |
165 | * I2C receive last byte and send no ACK |
166 | ****************************************/ |
166 | ****************************************/ |
167 | void I2C_ReceiveLastByte(void){ |
167 | void I2C_ReceiveLastByte(void) { |
168 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
168 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE); |
169 | } |
169 | } |
170 | 170 | ||
171 | /**************************************** |
171 | /**************************************** |
172 | * Reset I2C |
172 | * Reset I2C |
173 | ****************************************/ |
173 | ****************************************/ |
174 | void I2C_Reset(void) { |
174 | void I2C_Reset(void) { |
175 | // stop i2c bus |
175 | // stop i2c bus |
176 | I2C_Stop(TWI_STATE_MOTOR_TX); |
176 | I2C_Stop(TWI_STATE_MOTOR_TX); |
177 | twi_state = 0; |
177 | twi_state = 0; |
178 | motor_write = TWDR; |
178 | motor_write = TWDR; |
179 | motor_write = 0; |
179 | motor_write = 0; |
180 | motor_read = 0; |
180 | motor_read = 0; |
181 | TWCR = (1<<TWINT); // reset to original state incl. interrupt flag reset |
181 | TWCR = (1 << TWINT); // reset to original state incl. interrupt flag reset |
182 | TWAMR = 0; |
182 | TWAMR = 0; |
Line 183... | Line 183... | ||
183 | TWAR = 0; |
183 | TWAR = 0; |
184 | TWDR = 0; |
184 | TWDR = 0; |
185 | TWSR = 0; |
185 | TWSR = 0; |
186 | TWBR = 0; |
186 | TWBR = 0; |
- | 187 | I2C_init(); |
|
187 | I2C_init(); |
188 | I2C_Start(TWI_STATE_MOTOR_TX); |
188 | I2C_Start(TWI_STATE_MOTOR_TX); |
189 | } |
189 | } |
190 | |
190 | 191 | /**************************************** |
|
191 | /**************************************** |
192 | * I2C ISR |
192 | * I2C ISR |
193 | ****************************************/ |
- | 194 | ISR (TWI_vect) |
|
- | 195 | { |
|
193 | ****************************************/ |
196 | static uint8_t missing_motor = 0; |
194 | ISR (TWI_vect) { |
197 | switch (twi_state++) { // First i2c_start from SendMotorData() |
195 | static uint8_t missing_motor = 0; |
198 | // Master Transmit |
196 | switch (twi_state++) { // First i2c_start from SendMotorData() |
199 | case 0: // TWI_STATE_MOTOR_TX |
197 | // Master Transmit |
200 | // skip motor if not used in mixer |
198 | case 0: // TWI_STATE_MOTOR_TX |
201 | while ((Mixer.Motor[motor_write][MIX_THROTTLE] <= 0) && (motor_write |
199 | // skip motor if not used in mixer |
202 | < MAX_MOTORS)) |
200 | while((Mixer.Motor[motor_write][MIX_THROTTLE] <= 0) && (motor_write < MAX_MOTORS)) motor_write++; |
203 | motor_write++; |
201 | if(motor_write >= MAX_MOTORS) { // writing finished, read now |
204 | if (motor_write >= MAX_MOTORS) { // writing finished, read now |
202 | motor_write = 0; |
205 | motor_write = 0; |
203 | twi_state = TWI_STATE_MOTOR_RX; |
206 | twi_state = TWI_STATE_MOTOR_RX; |
204 | I2C_WriteByte(0x53 + (motor_read * 2)); // select slave adress in rx mode |
207 | I2C_WriteByte(0x53 + (motor_read * 2)); // select slave adress in rx mode |
- | 208 | } else |
|
205 | } |
209 | I2C_WriteByte(0x52 + (motor_write * 2)); // select slave adress in tx mode |
- | 210 | break; |
|
206 | else I2C_WriteByte(0x52 + (motor_write * 2)); // select slave adress in tx mode |
211 | case 1: // Send Data to Slave |
207 | break; |
212 | I2C_WriteByte(motor[motor_write].SetPoint); // transmit rotation rate setpoint |
208 | case 1: // Send Data to Slave |
213 | break; |
209 | I2C_WriteByte(motor[motor_write].SetPoint); // transmit rotation rate setpoint |
214 | case 2: // repeat case 0+1 for all motors |
210 | break; |
215 | if (TWSR == TW_MT_DATA_NACK) { // Data transmitted, NACK received |
211 | case 2: // repeat case 0+1 for all motors |
216 | if (!missing_motor) |
212 | if(TWSR == TW_MT_DATA_NACK) { // Data transmitted, NACK received |
217 | missing_motor = motor_write + 1; |
213 | if(!missing_motor) missing_motor = motor_write + 1; |
218 | if (++motor[motor_write].Error == 0) |
214 | if(++motor[motor_write].Error == 0) motor[motor_write].Error = 255; // increment error counter and handle overflow |
219 | motor[motor_write].Error = 255; // increment error counter and handle overflow |
215 | } |
220 | } |
216 | I2C_Stop(TWI_STATE_MOTOR_TX); |
221 | I2C_Stop(TWI_STATE_MOTOR_TX); |
217 | I2CTimeout = 10; |
222 | I2CTimeout = 10; |
218 | motor_write++; // next motor |
223 | motor_write++; // next motor |
- | 224 | I2C_Start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
|
219 | I2C_Start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
225 | break; |
220 | break; |
226 | // Master Receive Data |
221 | // Master Receive Data |
227 | case 3: |
222 | case 3: |
228 | if (TWSR != TW_MR_SLA_ACK) { // SLA+R transmitted, if not ACK received |
223 | if(TWSR != TW_MR_SLA_ACK) { // SLA+R transmitted, if not ACK received |
229 | // no response from the addressed slave received |
224 | // no response from the addressed slave received |
230 | motor[motor_read].Present = 0; |
225 | motor[motor_read].Present = 0; |
231 | motor_read++; // next motor |
226 | motor_read++; // next motor |
232 | if (motor_read >= MAX_MOTORS) |
227 | if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one |
233 | motor_read = 0; // restart reading of first motor if we have reached the last one |
228 | I2C_Stop(TWI_STATE_MOTOR_TX); |
234 | I2C_Stop(TWI_STATE_MOTOR_TX); |
229 | } else { |
235 | } else { |
230 | motor[motor_read].Present = ('1' - '-') + motor_read; |
236 | motor[motor_read].Present = ('1' - '-') + motor_read; |
231 | I2C_ReceiveByte(); //Transmit 1st byte |
237 | I2C_ReceiveByte(); //Transmit 1st byte |
232 | } |
238 | } |
233 | missingMotor = missing_motor; |
239 | missingMotor = missing_motor; |
234 | missing_motor = 0; |
240 | missing_motor = 0; |
235 | break; |
241 | break; |
- | 242 | case 4: //Read 1st byte and transmit 2nd Byte |
|
236 | case 4: //Read 1st byte and transmit 2nd Byte |
243 | motor[motor_read].Current = TWDR; |
237 | motor[motor_read].Current = TWDR; |
244 | I2C_ReceiveLastByte(); // nack |
238 | I2C_ReceiveLastByte(); // nack |
245 | break; |
239 | break; |
246 | case 5: |
240 | case 5: |
247 | //Read 2nd byte |
241 | //Read 2nd byte |
248 | motor[motor_read].MaxPWM = TWDR; |
242 | motor[motor_read].MaxPWM = TWDR; |
249 | motor_read++; // next motor |
243 | motor_read++; // next motor |
250 | if (motor_read >= MAX_MOTORS) |
244 | if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one |
251 | motor_read = 0; // restart reading of first motor if we have reached the last one |
245 | I2C_Stop(TWI_STATE_MOTOR_TX); |
252 | I2C_Stop(TWI_STATE_MOTOR_TX); |
246 | break; |
253 | break; |
247 | 254 | ||
248 | // Writing ADC values. |
255 | // Writing ADC values. |
249 | case 7: |
256 | case 7: |
250 | I2C_WriteByte(0x98); // Address the DAC |
257 | I2C_WriteByte(0x98); // Address the DAC |
251 | break; |
258 | break; |
252 | 259 | ||
253 | case 8: |
260 | case 8: |
254 | I2C_WriteByte(0x10 + (DACChannel << 1)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
261 | I2C_WriteByte(0x10 + (DACChannel << 1)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
255 | break; |
262 | break; |
256 | 263 | ||
257 | case 9: |
264 | case 9: |
258 | I2C_WriteByte(DACValues[DACChannel]); |
265 | I2C_WriteByte(DACValues[DACChannel]); |
259 | break; |
266 | break; |
260 | 267 | ||
261 | case 10: |
268 | case 10: |
262 | I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
269 | I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
263 | break; |
270 | break; |
264 | 271 | ||
265 | case 11: |
272 | case 11: |
266 | I2C_Stop(TWI_STATE_MOTOR_TX); |
273 | I2C_Stop(TWI_STATE_MOTOR_TX); |
267 | I2CTimeout = 10; |
274 | I2CTimeout = 10; |
268 | // repeat case 7...10 until all DAC Channels are updated |
275 | // repeat case 7...10 until all DAC Channels are updated |
269 | if(DACChannel < 2) { |
276 | if (DACChannel < 2) { |
270 | DACChannel ++; // jump to next channel |
277 | DACChannel++; // jump to next channel |
271 | I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel |
278 | I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel |
272 | } else { |
279 | } else { |
273 | DACChannel = 0; // reset dac channel counter |
280 | DACChannel = 0; // reset dac channel counter |
274 | } |
281 | } |
275 | break; |
282 | break; |
Line 276... | Line 283... | ||
276 | 283 | ||
277 | default: |
284 | default: |
278 | I2C_Stop(TWI_STATE_MOTOR_TX); |
285 | I2C_Stop(TWI_STATE_MOTOR_TX); |
279 | I2CTimeout = 10; |
286 | I2CTimeout = 10; |
280 | motor_write = 0; |
287 | motor_write = 0; |
281 | motor_read = 0; |
288 | motor_read = 0; |
282 | } |
289 | } |
283 | } |
290 | } |
284 | 291 | ||
285 | extern void twi_diagnostics(void) { |
292 | extern void twi_diagnostics(void) { |
286 | // Check connected BL-Ctrls |
293 | // Check connected BL-Ctrls |
287 | uint8_t i; |
294 | uint8_t i; |
288 | 295 | ||
289 | printf("\n\rFound BL-Ctrl: "); |
296 | printf("\n\rFound BL-Ctrl: "); |
290 | 297 | ||
291 | for(i = 0; i < MAX_MOTORS; i++) { |
298 | for (i = 0; i < MAX_MOTORS; i++) { |
292 | motor[i].SetPoint = 0; |
299 | motor[i].SetPoint = 0; |
293 | } |
300 | } |
294 | 301 | ||
- | 302 | I2C_Start(TWI_STATE_MOTOR_TX); |
|
295 | I2C_Start(TWI_STATE_MOTOR_TX); |
303 | _delay_ms(2); |
296 | _delay_ms(2); |
304 | |
297 | 305 | motor_read = 0; // read the first I2C-Data |
|
298 | motor_read = 0; // read the first I2C-Data |
306 | |
- | 307 | for (i = 0; i < MAX_MOTORS; i++) { |
|
299 | 308 | I2C_Start(TWI_STATE_MOTOR_TX); |
|
300 | for(i = 0; i < MAX_MOTORS; i++) { |
309 | _delay_ms(2); |
301 | I2C_Start(TWI_STATE_MOTOR_TX); |
310 | if (motor[i].Present) |