Rev 1662 | Rev 1666 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1662 | Rev 1665 | ||
---|---|---|---|
Line 55... | Line 55... | ||
55 | #include <util/twi.h> |
55 | #include <util/twi.h> |
56 | #include "eeprom.h" |
56 | #include "eeprom.h" |
57 | #include "twimaster.h" |
57 | #include "twimaster.h" |
58 | #include "fc.h" |
58 | #include "fc.h" |
59 | #include "analog.h" |
59 | #include "analog.h" |
- | 60 | #include "uart.h" |
|
Line 60... | Line 61... | ||
60 | 61 | ||
61 | volatile uint8_t twi_state = TWI_STATE_MOTOR_TX; |
62 | volatile uint8_t twi_state = TWI_STATE_MOTOR_TX; |
62 | volatile uint8_t dac_channel = 0; |
63 | volatile uint8_t dac_channel = 0; |
63 | volatile uint8_t motor_write = 0; |
64 | volatile uint8_t motor_write = 0; |
Line 347... | Line 348... | ||
347 | break; |
348 | break; |
Line 348... | Line 349... | ||
348 | 349 | ||
349 | case 22: |
350 | case 22: |
350 | I2C_Stop(TWI_STATE_MOTOR_TX); |
351 | I2C_Stop(TWI_STATE_MOTOR_TX); |
351 | I2CTimeout = 10; |
352 | I2CTimeout = 10; |
352 | // repeat case 7...10 until all DAC Channels are updated |
353 | // repeat case 18...22 until all DAC Channels are updated |
353 | if(dac_channel < 2) |
354 | if(dac_channel < 2) |
354 | { |
355 | { |
355 | dac_channel ++; // jump to next channel |
356 | dac_channel ++; // jump to next channel |
356 | I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel |
357 | I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel |
357 | } |
358 | } |
358 | else |
359 | else |
359 | { // data to last motor send |
360 | { |
- | 361 | dac_channel = 0; // reset dac channel counter |
|
360 | dac_channel = 0; // reset dac channel counter |
362 | BLFlags |= BLFLAG_TX_COMPLETE; |
361 | } |
363 | } |
Line 362... | Line 364... | ||
362 | break; |
364 | break; |
363 | 365 | ||
Line 375... | Line 377... | ||
375 | 377 | ||
376 | uint8_t I2C_WriteBLConfig(uint8_t motor) |
378 | uint8_t I2C_WriteBLConfig(uint8_t motor) |
377 | { |
379 | { |
Line 378... | Line 380... | ||
378 | uint8_t i; |
380 | uint8_t i; |
379 | 381 | ||
380 | if(MotorenEin) return(0); // not when motors are running! |
382 | if(MotorenEin || PC_MotortestActive) return(BLCONFIG_ERR_MOTOR_RUNNING); // not when motors are running! |
381 | if(motor > MAX_MOTORS) return (0); // motor does not exist! |
383 | if(motor > MAX_MOTORS) return (BLCONFIG_ERR_MOTOR_NOT_EXIST); // motor does not exist! |
382 | if(motor) |
384 | if(motor) |
383 | { |
385 | { |
384 | if(!(Motor[motor-1].State & MOTOR_STATE_PRESENT_MASK)) return(0); // motor does not exist! |
386 | if(!(Motor[motor-1].State & MOTOR_STATE_PRESENT_MASK)) return(BLCONFIG_ERR_MOTOR_NOT_EXIST); // motor does not exist! |
385 | if(!(Motor[motor-1].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) return(0); // not a new BL! |
387 | if(!(Motor[motor-1].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) return(BLCONFIG_ERR_HW_NOT_COMPATIBLE); // not a new BL! |
386 | } |
388 | } |
387 | // check BL configuration to send |
389 | // check BL configuration to send |
388 | if(BLConfig.Revision != BLCONFIG_REVISION) return (0); // bad revison |
390 | if(BLConfig.Revision != BLCONFIG_REVISION) return (BLCONFIG_ERR_SW_NOT_COMPATIBLE); // bad revison |
Line 389... | Line 391... | ||
389 | i = RAM_Checksum((uint8_t*)&BLConfig, sizeof(BLConfig_t) - 1); |
391 | i = RAM_Checksum((uint8_t*)&BLConfig, sizeof(BLConfig_t) - 1); |
Line 390... | Line 392... | ||
390 | if(i != BLConfig.crc) return(0); // bad checksum |
392 | if(i != BLConfig.crc) return(BLCONFIG_ERR_CHECKSUM); // bad checksum |
391 | 393 | ||
Line 398... | Line 400... | ||
398 | } |
400 | } |
399 | else //only one specific motor |
401 | else //only one specific motor |
400 | { |
402 | { |
401 | BLConfig_WriteMask = 0x0001<<(motor-1); |
403 | BLConfig_WriteMask = 0x0001<<(motor-1); |
402 | } |
404 | } |
403 | - | ||
404 | for(i = 0; i < MAX_MOTORS; i++) |
405 | for(i = 0; i < MAX_MOTORS; i++) |
405 | { |
406 | { |
406 | if((0x0001<<i) & BLConfig_WriteMask) |
407 | if((0x0001<<i) & BLConfig_WriteMask) |
407 | { |
408 | { |
408 | Motor[i].SetPoint = 0; |
409 | Motor[i].SetPoint = 0; |
409 | Motor[i].SetPointLowerBits = 0; |
410 | Motor[i].SetPointLowerBits = 0; |
410 | } |
411 | } |
411 | } |
412 | } |
- | 413 | ||
412 | motor_write = 0; |
414 | motor_write = 0; |
413 | // needs at least MAX_MOTORS loops of 2 ms (12*2ms = 24ms) |
415 | // needs at least MAX_MOTORS loops of 2 ms (12*2ms = 24ms) |
414 | do |
416 | do |
415 | { |
417 | { |
416 | I2C_Start(TWI_STATE_MOTOR_TX); // start an i2c transmission |
418 | I2C_Start(TWI_STATE_MOTOR_TX); // start an i2c transmission |
417 | while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer |
419 | while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer |
418 | }while(BLConfig_WriteMask); // repeat until the BL config has been sent |
420 | }while(BLConfig_WriteMask); // repeat until the BL config has been sent |
419 | return(1); |
421 | return(BLCONFIG_SUCCESS); |
420 | } |
422 | } |
Line 421... | Line 423... | ||
421 | 423 | ||
422 | uint8_t I2C_ReadBLConfig(uint8_t motor) |
424 | uint8_t I2C_ReadBLConfig(uint8_t motor) |
423 | { |
425 | { |
Line 424... | Line 426... | ||
424 | uint8_t i; |
426 | uint8_t i; |
425 | 427 | ||
426 | if(MotorenEin) return(0); // not when motors are running! |
428 | if(MotorenEin || PC_MotortestActive) return(BLCONFIG_ERR_MOTOR_RUNNING); // not when motors are running! |
427 | if((motor == 0) || (motor > MAX_MOTORS)) return (0); // motor does not exist! |
429 | if((motor == 0) || (motor > MAX_MOTORS)) return (BLCONFIG_ERR_MOTOR_NOT_EXIST); // motor does not exist! |
- | 430 | if(!(Motor[motor-1].State & MOTOR_STATE_PRESENT_MASK)) return(BLCONFIG_ERR_MOTOR_NOT_EXIST); // motor does not exist! |
|
428 | if(!(Motor[motor-1].State & MOTOR_STATE_PRESENT_MASK)) return(0); // motor does not exist! |
431 | if(!(Motor[motor-1].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) return(BLCONFIG_ERR_HW_NOT_COMPATIBLE); // not a new BL! |
Line 429... | Line 432... | ||
429 | if(!(Motor[motor-1].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) return(0); // not a new BL! |
432 | |
430 | while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer |
433 | while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer |
- | 434 | ||
431 | 435 | // prepare the bitmask |
|
432 | // prepare the bitmask |
436 | BLConfig_ReadMask = 0x0001<<(motor-1); |
433 | BLConfig_ReadMask = 0x0001<<(motor-1); |
437 | |
434 | for(i = 0; i < MAX_MOTORS; i++) |
438 | for(i = 0; i < MAX_MOTORS; i++) |
435 | { |
439 | { |
436 | if((0x0001<<i) & BLConfig_ReadMask) |
440 | if((0x0001<<i) & BLConfig_ReadMask) |
437 | { |
441 | { |
438 | Motor[i].SetPoint = 0; |
442 | Motor[i].SetPoint = 0; |
- | 443 | Motor[i].SetPointLowerBits = 0; |
|
439 | Motor[i].SetPointLowerBits = 0; |
444 | } |
440 | } |
445 | } |
441 | } |
446 | |
442 | motor_read = 0; |
447 | motor_read = 0; |
443 | BLConfig.Revision = 0; // bad revision |
448 | BLConfig.Revision = 0; // bad revision |
444 | BLConfig.crc = 0; // bad checksum |
449 | BLConfig.crc = 0; // bad checksum |
445 | // needs at least MAX_MOTORS loops of 2 ms (12*2ms = 24ms) |
450 | // needs at least MAX_MOTORS loops of 2 ms (12*2ms = 24ms) |
446 | do |
451 | do |
447 | { |
452 | { |
448 | I2C_Start(TWI_STATE_MOTOR_TX); // start an i2c transmission |
453 | I2C_Start(TWI_STATE_MOTOR_TX); // start an i2c transmission |
449 | while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer |
454 | while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer |
450 | }while(BLConfig_ReadMask); // repeat until the BL config has been received from all motors |
455 | }while(BLConfig_ReadMask); // repeat until the BL config has been received from all motors |
451 | // validate result |
456 | // validate result |
452 | if(BLConfig.Revision != BLCONFIG_REVISION) return (0); // bad revison |
457 | if(BLConfig.Revision != BLCONFIG_REVISION) return (BLCONFIG_ERR_SW_NOT_COMPATIBLE); // bad revison |
453 | i = RAM_Checksum((uint8_t*)&BLConfig, sizeof(BLConfig_t) - 1); |
458 | i = RAM_Checksum((uint8_t*)&BLConfig, sizeof(BLConfig_t) - 1); |
- | 459 | if(i != BLConfig.crc) return(BLCONFIG_ERR_CHECKSUM); // bad checksum |