Rev 2426 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2426 | Rev 2443 | ||
---|---|---|---|
Line 58... | Line 58... | ||
58 | #include "twimaster.h" |
58 | #include "twimaster.h" |
59 | #include "fc.h" |
59 | #include "fc.h" |
60 | #include "analog.h" |
60 | #include "analog.h" |
61 | #include "uart.h" |
61 | #include "uart.h" |
62 | #include "timer0.h" |
62 | #include "timer0.h" |
- | 63 | #include "main.h" |
|
Line 63... | Line 64... | ||
63 | 64 | ||
64 | volatile uint8_t twi_state = TWI_STATE_MOTOR_TX; |
65 | volatile uint8_t twi_state = TWI_STATE_MOTOR_TX; |
65 | volatile uint8_t dac_channel = 0; |
66 | volatile uint8_t dac_channel = 0; |
66 | volatile uint8_t motor_write = 0; |
67 | volatile uint8_t motor_write = 0; |
Line 128... | Line 129... | ||
128 | Motor[i].State = 0; |
129 | Motor[i].State = 0; |
129 | Motor[i].ReadMode = BL_READMODE_STATUS; |
130 | Motor[i].ReadMode = BL_READMODE_STATUS; |
130 | Motor[i].Current = 0; |
131 | Motor[i].Current = 0; |
131 | Motor[i].MaxPWM = 0; |
132 | Motor[i].MaxPWM = 0; |
132 | Motor[i].Temperature = 0; |
133 | Motor[i].Temperature = 0; |
- | 134 | Motor[i].NotReadyCnt = 0; |
|
133 | } |
135 | } |
134 | sei(); |
136 | sei(); |
135 | SREG = sreg; |
137 | SREG = sreg; |
136 | } |
138 | } |
Line 286... | Line 288... | ||
286 | MissingMotor = missing_motor; |
288 | MissingMotor = missing_motor; |
287 | missing_motor = 0; |
289 | missing_motor = 0; |
288 | break; |
290 | break; |
289 | case 6: // receive bytes |
291 | case 6: // receive bytes |
290 | *pBuff = TWDR; |
292 | *pBuff = TWDR; |
291 | pBuff++; |
293 | pBuff++; // set Pointer to next element : Motor[].Current,Motor[].Temperature |
292 | BuffLen--; |
294 | BuffLen--; |
293 | if(BuffLen>1) |
295 | if(BuffLen>1) |
294 | { |
296 | { |
295 | I2C_ReceiveByte(); // read next byte |
297 | I2C_ReceiveByte(); // read next byte |
296 | } |
298 | } |
297 | else if (BuffLen == 1) |
299 | else if (BuffLen == 1) |
298 | { |
300 | { |
299 | I2C_ReceiveLastByte(); // read last byte |
301 | I2C_ReceiveLastByte(); // read last byte |
300 | } |
302 | } |
301 | else // nothing left |
303 | else // nothing left -> ready |
302 | { |
304 | { |
303 | if(BLFlags & BLFLAG_READ_VERSION) |
305 | if(BLFlags & BLFLAG_READ_VERSION) |
304 | { |
306 | { |
305 | // if(!(FC_StatusFlags & FC_STATUS_MOTOR_RUN) && ((Motor[motor_read].MaxPWM & 252) == 248)) Motor[motor_read].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK; |
- | |
306 | // else Motor[motor_read].Version = 0; |
- | |
307 | if(!(FC_StatusFlags & FC_STATUS_MOTOR_RUN)) |
307 | if(!(FC_StatusFlags & FC_STATUS_MOTOR_RUN)) |
308 | { |
308 | { |
309 | if((Motor[motor_read].MaxPWM & 252) == 248) Motor[motor_read].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK; |
309 | if((Motor[motor_read].MaxPWM & 252) == 248) Motor[motor_read].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK; |
310 | else Motor[motor_read].Version = 0; |
310 | else Motor[motor_read].Version = 0; |
311 | if(Motor[motor_read].MaxPWM == 248) Motor[motor_read].Version |= (MOTOR_STATE_FAST_MODE | MOTOR_STATE_BL30); |
311 | if(Motor[motor_read].MaxPWM == 248) Motor[motor_read].Version |= (MOTOR_STATE_FAST_MODE | MOTOR_STATE_BL30); |
312 | else |
312 | else |
313 | if(Motor[motor_read].MaxPWM == 249) Motor[motor_read].Version |= MOTOR_STATE_BL30; |
313 | if(Motor[motor_read].MaxPWM == 249) Motor[motor_read].Version |= MOTOR_STATE_BL30; |
314 | } |
314 | } |
315 | } |
315 | } |
- | 316 | ||
- | 317 | if(FC_StatusFlags & FC_STATUS_FLY) |
|
- | 318 | { |
|
- | 319 | // Starting -> 40 |
|
- | 320 | // I2C-Setpoint is zero -> 250 |
|
- | 321 | // 255 -> Running and no Redundancy |
|
- | 322 | // 254 -> Running and active Redundancy |
|
- | 323 | if(Motor[motor_read].MaxPWM < 254) |
|
- | 324 | { |
|
- | 325 | Motor[motor_read].NotReadyCnt++; |
|
- | 326 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
|
- | 327 | SpeakHoTT = SPEAK_ERR_MOTOR; |
|
- | 328 | #endif |
|
- | 329 | } |
|
- | 330 | } |
|
316 | if(++motor_read >= MAX_MOTORS) |
331 | if(++motor_read >= MAX_MOTORS) |
317 | { |
332 | { |
318 | motor_read = 0; // restart from beginning |
333 | motor_read = 0; // restart from beginning |
319 | BLConfig_ReadMask = 0; // reset read configuration bitmask |
334 | BLConfig_ReadMask = 0; // reset read configuration bitmask |
320 | if(++motor_read_temperature >= MAX_MOTORS) |
335 | if(++motor_read_temperature >= MAX_MOTORS) |