Subversion Repositories FlightCtrl

Rev

Rev 1654 | Rev 1662 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1654 Rev 1657
Line 7... Line 7...
7 
volatile unsigned char motor = 0;
 7 
volatile unsigned char motor = 0;
8 
volatile unsigned char motorread = 0,MissingMotor = 0;
 8 
volatile unsigned char motorread = 0,MissingMotor = 0;
9 
volatile unsigned char BLFlags = 0;
 9 
volatile unsigned char BLFlags = 0;
Line 10... Line 10...
10 
 
 10 
 
11 
MotorData_t Motor[MAX_MOTORS];
 - 
 
Line 12... Line 11...
12 
BLConfig_t BLConfig[MAX_MOTORS];
 11 
MotorData_t Motor[MAX_MOTORS];
Line 13... Line -...
13 
 
 - 
 
14 
 
 - 
 
15 
unsigned int I2CError = 0;
 12 
 
16 
 
 13 
 
17 
#define BL_CONF_SYNC_BYTE '#'
 14 
unsigned int I2CError = 0;
18 
 
 15 
 
19 
//############################################################################
 16 
//############################################################################
Line 64... Line 61...
64 
 
 61 
 
65 
//############################################################################
 62 
//############################################################################
66 
SIGNAL (TWI_vect)
 63 
SIGNAL (TWI_vect)
67 
//############################################################################
 64 
//############################################################################
68 
{
 65 
{
69 
        static unsigned char missing_motor = 0, byte_counter = 0, crc = 0, read_more = 0, motorread_temperature = 0;
 66 
        static unsigned char missing_motor = 0, byte_counter = 0, read_more = 0, motorread_temperature = 0;
70 
        static unsigned char *pTxBuff;
 67 
        static unsigned char *pTxBuff;
71 
J4High;
 68 
J4High;
72 
        switch(twi_state++)
 69 
        switch(twi_state++)
73 
        {
 70 
        {
Line 99... Line 96...
99 
                        i2c_write_byte((Motor[motor].SetPointLowerBits << 1) & 0x07); // send the lower bits of setpoint
 96 
                        i2c_write_byte((Motor[motor].SetPointLowerBits << 1) & 0x07); // send the lower bits of setpoint
100 
                        // transmit config only on demand and the motors are not running and only for one motor per round trip
 97 
                        // transmit config only on demand and the motors are not running and only for one motor per round trip
101 
                        if( (BLFlags & BLFLAG_SEND_CONFIG) && (motor == motorread))
 98 
                        if( (BLFlags & BLFLAG_SEND_CONFIG) && (motor == motorread))
102 
                        {       // prepare sending of configuration
 99 
                        {       // prepare sending of configuration
103 
                                byte_counter = 0;       // reset send byte counter
 100 
                                byte_counter = 0;       // reset send byte counter
104 
                                crc = 0xAA; // init checksum
 - 
 
105 
                        }
 101 
                        }
106 
                        else
 102 
                        else
107 
                        {       // jump to state for end of transmission for that motor
 103 
                        {       // jump to state for end of transmission for that motor
108 
                                twi_state = 4;
 104 
                                twi_state = 4;
109 
                        }
 105 
                        }
110 
                        break;
 106 
                        break;
111 
                case 3:
 107 
                case 3:
112 
                        if(!byte_counter) // first byte?
 108 
                        if(!byte_counter) // first byte?
113 
                        {
 109 
                        {
114 
                                i2c_write_byte(BL_CONF_SYNC_BYTE);
 - 
 
115 
                                crc += BL_CONF_SYNC_BYTE; // update crc
 - 
 
116 
                                pTxBuff = (uint8_t*)&BLConfig[motor]; // select configuration for motor
 110 
                                pTxBuff = (uint8_t*)&BLConfig[motor]; // select configuration for motor
- 
 
 111 
                                i2c_write_byte(pTxBuff[byte_counter]);
117 
                                twi_state = 3; // keep state 3
 112 
                                twi_state = 3; // keep state 3
118 
                        }
 113 
                        }
119 
                        else if(byte_counter == sizeof(BLConfig_t) + 1)
 - 
 
120 
                        {       // send crc byte at the end
 - 
 
121 
                                i2c_write_byte(crc);
 - 
 
122 
                                twi_state = 3; // keep state 3
 - 
 
123 
                        }
 - 
 
124 
                        else if(byte_counter > sizeof(BLConfig_t) + 1)
 114 
                        else if(byte_counter >= sizeof(BLConfig_t))
125 
                        {
 115 
                        {
126 
                                i2c_write_byte(0);
 116 
                                i2c_write_byte(0);
127 
                                // jump to case 4
 117 
                                // jump to case 4
128 
                        }
 118 
                        }
129 
            else // transmit configuration to BLs
 119 
            else // transmit configuration to BLs
130 
                        {
 120 
                        {
131 
                                i2c_write_byte(pTxBuff[byte_counter-1]);        // submit next byte
 121 
                                i2c_write_byte(pTxBuff[byte_counter]);  // submit next byte
132 
                                crc += pTxBuff[byte_counter-1];                         // update crc
 - 
 
133 
                                twi_state = 3;// keep state 3
 122 
                                twi_state = 3;// keep state 3
134 
                        }
 123 
                        }
135 
                        byte_counter++; // next byte
 124 
                        byte_counter++; // next byte
136 
                        break;
 125 
                        break;
137 
        case 4:
 126 
        case 4: