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; |