Rev 1775 | Rev 1868 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1775 | Rev 1821 | ||
---|---|---|---|
Line 189... | Line 189... | ||
189 | } |
189 | } |
Line 190... | Line 190... | ||
190 | 190 | ||
191 | /**************************************** |
191 | /**************************************** |
192 | * I2C ISR |
192 | * I2C ISR |
193 | ****************************************/ |
193 | ****************************************/ |
- | 194 | ISR (TWI_vect) |
|
194 | ISR (TWI_vect) { |
195 | { |
195 | static uint8_t missing_motor = 0; |
196 | static uint8_t missing_motor = 0; |
196 | switch (twi_state++) { // First i2c_start from SendMotorData() |
197 | switch (twi_state++) { // First i2c_start from SendMotorData() |
197 | // Master Transmit |
198 | // Master Transmit |
198 | case 0: // TWI_STATE_MOTOR_TX |
199 | case 0: // TWI_STATE_MOTOR_TX |
199 | // skip motor if not used in mixer |
200 | // skip motor if not used in mixer |
- | 201 | while ((Mixer.Motor[motor_write][MIX_THROTTLE] <= 0) && (motor_write |
|
- | 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 |
205 | } |
208 | } else |
206 | else I2C_WriteByte(0x52 + (motor_write * 2)); // select slave adress in tx mode |
209 | I2C_WriteByte(0x52 + (motor_write * 2)); // select slave adress in tx mode |
207 | break; |
210 | break; |
208 | case 1: // Send Data to Slave |
211 | case 1: // Send Data to Slave |
209 | I2C_WriteByte(motor[motor_write].SetPoint); // transmit rotation rate setpoint |
212 | I2C_WriteByte(motor[motor_write].SetPoint); // transmit rotation rate setpoint |
210 | break; |
213 | break; |
211 | case 2: // repeat case 0+1 for all motors |
214 | case 2: // repeat case 0+1 for all motors |
- | 215 | if (TWSR == TW_MT_DATA_NACK) { // Data transmitted, NACK received |
|
212 | if(TWSR == TW_MT_DATA_NACK) { // Data transmitted, NACK received |
216 | if (!missing_motor) |
- | 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 |
Line 222... | Line 227... | ||
222 | case 3: |
227 | case 3: |
223 | if(TWSR != TW_MR_SLA_ACK) { // SLA+R transmitted, if not ACK received |
228 | if (TWSR != TW_MR_SLA_ACK) { // SLA+R transmitted, if not ACK received |
224 | // no response from the addressed slave received |
229 | // no response from the addressed slave received |
225 | motor[motor_read].Present = 0; |
230 | motor[motor_read].Present = 0; |
226 | motor_read++; // next motor |
231 | 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 | } |
Line 239... | Line 245... | ||
239 | break; |
245 | break; |
240 | case 5: |
246 | case 5: |
241 | //Read 2nd byte |
247 | //Read 2nd byte |
242 | motor[motor_read].MaxPWM = TWDR; |
248 | motor[motor_read].MaxPWM = TWDR; |
243 | motor_read++; // next motor |
249 | 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; |
Line 247... | Line 254... | ||
247 | 254 | ||
248 | // Writing ADC values. |
255 | // Writing ADC values. |
Line 298... | Line 305... | ||
298 | motor_read = 0; // read the first I2C-Data |
305 | motor_read = 0; // read the first I2C-Data |
Line 299... | Line 306... | ||
299 | 306 | ||
300 | for(i = 0; i < MAX_MOTORS; i++) { |
307 | for (i = 0; i < MAX_MOTORS; i++) { |
301 | I2C_Start(TWI_STATE_MOTOR_TX); |
308 | I2C_Start(TWI_STATE_MOTOR_TX); |
302 | _delay_ms(2); |
309 | _delay_ms(2); |
- | 310 | if (motor[i].Present) |
|
303 | if(motor[i].Present) printf("%d ",i+1); |
311 | printf("%d ",i+1); |
Line 304... | Line 312... | ||
304 | } |
312 | } |
305 | 313 | ||
- | 314 | for (i = 0; i < MAX_MOTORS; i++) { |
|
306 | for(i = 0; i < MAX_MOTORS; i++) { |
315 | if (!motor[i].Present && Mixer.Motor[i][MIX_THROTTLE] > 0) |
307 | if(!motor[i].Present && Mixer.Motor[i][MIX_THROTTLE] > 0) printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i + 1); |
316 | printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i + 1); |
308 | motor[i].Error = 0; |
317 | motor[i].Error = 0; |