Rev 1648 | Rev 1651 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1648 | Rev 1650 | ||
---|---|---|---|
1 | /*############################################################################ |
1 | /*############################################################################ |
2 | ############################################################################*/ |
2 | ############################################################################*/ |
3 | 3 | ||
4 | #include "main.h" |
4 | #include "main.h" |
5 | 5 | ||
6 | volatile unsigned char twi_state = 0; |
6 | volatile unsigned char twi_state = 0; |
7 | unsigned char motor = 0; |
7 | volatile unsigned char motor = 0; |
8 | unsigned char motorread = 0,MissingMotor = 0; |
8 | volatile unsigned char motorread = 0,MissingMotor = 0; |
9 | unsigned char BLFlags = 0; |
9 | volatile unsigned char BLFlags = 0; |
10 | 10 | ||
11 | MotorData_t Motor[MAX_MOTORS]; |
11 | MotorData_t Motor[MAX_MOTORS]; |
12 | BLConfig_t BLConfig[MAX_MOTORS]; |
12 | BLConfig_t BLConfig[MAX_MOTORS]; |
13 | - | ||
14 | unsigned char *pTxBuff = (uint8_t*)&BLConfig[0]; |
13 | |
15 | 14 | ||
16 | unsigned int I2CError = 0; |
15 | unsigned int I2CError = 0; |
17 | 16 | ||
18 | #define BL_CONF_SYNC_BYTE '#' |
17 | #define BL_CONF_SYNC_BYTE '#' |
19 | 18 | ||
20 | //############################################################################ |
19 | //############################################################################ |
21 | //Initzialisieren der I2C (TWI) Schnittstelle |
20 | //Initzialisieren der I2C (TWI) Schnittstelle |
22 | void i2c_init(void) |
21 | void i2c_init(void) |
23 | //############################################################################ |
22 | //############################################################################ |
24 | { |
23 | { |
25 | unsigned char i; |
24 | unsigned char i; |
26 | 25 | ||
27 | TWSR = 0; |
26 | TWSR = 0; |
28 | TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
27 | TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
29 | 28 | ||
30 | for(i=0; i < MAX_MOTORS; i++) |
29 | for(i=0; i < MAX_MOTORS; i++) |
31 | { |
30 | { |
32 | Motor[i].Version = 0; |
31 | Motor[i].Version = 0; |
33 | Motor[i].SetPoint = 0; |
32 | Motor[i].SetPoint = 0; |
34 | Motor[i].SetPointLowerBits = 0; |
33 | Motor[i].SetPointLowerBits = 0; |
35 | Motor[i].State = 0; |
34 | Motor[i].State = 0; |
36 | Motor[i].Current = 0; |
35 | Motor[i].Current = 0; |
37 | Motor[i].MaxPWM = 0; |
36 | Motor[i].MaxPWM = 0; |
38 | BLConfig[i].SetMask = MASK_SET_PWM_SCALING|MASK_SET_CURRENT_LIMIT|MASK_SET_TEMP_LIMIT|MASK_SET_CURRENT_SCALING|MASK_SET_BITCONFIG; |
37 | BLConfig[i].SetMask = MASK_SET_PWM_SCALING|MASK_SET_CURRENT_LIMIT|MASK_SET_TEMP_LIMIT|MASK_SET_CURRENT_SCALING|MASK_SET_BITCONFIG; |
39 | BLConfig[i].PwmScaling = 255; // MaxPWM |
38 | BLConfig[i].PwmScaling = 255; // MaxPWM |
40 | BLConfig[i].CurrentLimit = 30; // Current Limit in A |
39 | BLConfig[i].CurrentLimit = 30; // Current Limit in A |
41 | BLConfig[i].TempLimit = 99; // Temperature Limit in °C |
40 | BLConfig[i].TempLimit = 99; // Temperature Limit in °C |
42 | BLConfig[i].CurrentScaling = 64; // Current Scaling |
41 | BLConfig[i].CurrentScaling = 64; // Current Scaling |
43 | BLConfig[i].BitConfig = 0; // BitConfig |
42 | BLConfig[i].BitConfig = 0; // BitConfig |
44 | } |
43 | } |
45 | } |
44 | } |
46 | 45 | ||
47 | void i2c_reset(void) |
46 | void i2c_reset(void) |
48 | //############################################################################ |
47 | //############################################################################ |
49 | { |
48 | { |
50 | I2C_Stop(); |
49 | I2C_Stop(); |
51 | twi_state = 0; |
50 | twi_state = 0; |
52 | motor = TWDR; |
51 | motor = TWDR; |
53 | motor = 0; |
52 | motor = 0; |
54 | TWCR = 0x80; |
53 | TWCR = 0x80; |
55 | TWAMR = 0; |
54 | TWAMR = 0; |
56 | TWAR = 0; |
55 | TWAR = 0; |
57 | TWDR = 0; |
56 | TWDR = 0; |
58 | TWSR = 0; |
57 | TWSR = 0; |
59 | TWBR = 0; |
58 | TWBR = 0; |
60 | i2c_init(); |
59 | i2c_init(); |
61 | I2C_Start(); |
60 | I2C_Start(); |
62 | i2c_write_byte(0); |
61 | i2c_write_byte(0); |
63 | } |
62 | } |
64 | 63 | ||
65 | 64 | ||
66 | //############################################################################ |
65 | //############################################################################ |
67 | SIGNAL (TWI_vect) |
66 | SIGNAL (TWI_vect) |
68 | //############################################################################ |
67 | //############################################################################ |
69 | { |
68 | { |
70 | static unsigned char missing_motor = 0, byte_counter = 0, crc = 0, read_more = 0, motorread_temperature = 0; |
69 | static unsigned char missing_motor = 0, byte_counter = 0, crc = 0, read_more = 0, motorread_temperature = 0; |
- | 70 | static unsigned char *pTxBuff; |
|
- | 71 | ||
71 | J4High; |
72 | J4High; |
72 | switch(twi_state++) |
73 | switch(twi_state++) |
73 | { |
74 | { |
74 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
75 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
75 | // Writing the Data |
76 | // Writing the Data |
76 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
77 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
77 | case 0: |
78 | case 0: |
78 | while(Mixer.Motor[motor][MIX_GAS] <= 0 && motor < MAX_MOTORS) motor++; // skip if not used |
79 | while(Mixer.Motor[motor][MIX_GAS] <= 0 && motor < MAX_MOTORS) motor++; // skip if not used |
79 | if(motor == MAX_MOTORS) // writing finished -> now read |
80 | if(motor == MAX_MOTORS) // writing finished -> now read |
80 | { |
81 | { |
81 | motor = 0; |
82 | motor = 0; |
82 | twi_state = 5; |
83 | twi_state = 5; |
83 | i2c_write_byte(0x53+(motorread*2)); |
84 | i2c_write_byte(0x53+(motorread*2)); |
84 | } |
85 | } |
85 | else i2c_write_byte(0x52+(motor*2)); |
86 | else i2c_write_byte(0x52+(motor*2)); |
86 | break; |
87 | break; |
87 | case 1: |
88 | case 1: |
88 | i2c_write_byte(Motor[motor].SetPoint); |
89 | i2c_write_byte(Motor[motor].SetPoint); |
89 | if(!(Motor[motor].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) ) |
90 | if(!(Motor[motor].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) ) |
90 | { |
91 | { |
91 | twi_state = 4; //jump over sending more data |
92 | twi_state = 4; //jump over sending more data |
92 | } |
93 | } |
93 | else if(!(Motor[motor].SetPointLowerBits || (BLFlags & BLFLAG_SEND_CONFIG))) |
94 | else if(!(Motor[motor].SetPointLowerBits || (BLFlags & BLFLAG_SEND_CONFIG))) |
94 | { |
95 | { |
95 | twi_state = 4; // skip state |
96 | twi_state = 4; // skip state |
96 | } |
97 | } |
97 | break; |
98 | break; |
98 | case 2: // lower bits of setpoint (higher resolution) |
99 | case 2: // lower bits of setpoint (higher resolution) |
99 | i2c_write_byte((Motor[motor].SetPointLowerBits << 1) & 0x07); // send the lower bits of setpoint |
100 | 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 |
101 | // 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) && !MotorenEin && (motor == motorread)) |
102 | if( (BLFlags & BLFLAG_SEND_CONFIG) && !MotorenEin && (motor == motorread)) |
102 | { // prepare sending of configuration |
103 | { // prepare sending of configuration |
103 | byte_counter = 0; // reset send byte counter |
104 | byte_counter = 0; // reset send byte counter |
104 | crc = 0xAA; // init checksum |
105 | crc = 0xAA; // init checksum |
105 | } |
106 | } |
106 | else |
107 | else |
107 | { // jump to state for end of transmission for that motor |
108 | { // jump to state for end of transmission for that motor |
108 | twi_state = 4; |
109 | twi_state = 4; |
109 | } |
110 | } |
110 | break; |
111 | break; |
111 | case 3: |
112 | case 3: |
112 | if(!byte_counter) // first byte? |
113 | if(!byte_counter) // first byte? |
113 | { |
114 | { |
114 | i2c_write_byte(BL_CONF_SYNC_BYTE); |
115 | i2c_write_byte(BL_CONF_SYNC_BYTE); |
115 | crc += BL_CONF_SYNC_BYTE; // update crc |
116 | crc += BL_CONF_SYNC_BYTE; // update crc |
116 | pTxBuff = (uint8_t*)&BLConfig[motor]; // select configuration for motor |
117 | pTxBuff = (uint8_t*)&BLConfig[motor]; // select configuration for motor |
117 | } |
118 | } |
118 | else if(byte_counter >= sizeof(BLConfig_t)+1) // last byte? |
119 | else if(byte_counter >= sizeof(BLConfig_t)+1) // last byte? |
119 | { // send crc byte at the end |
120 | { // send crc byte at the end |
120 | i2c_write_byte(crc); |
121 | i2c_write_byte(crc); |
121 | } |
122 | } |
122 | else // transmit configuration to BLs |
123 | else // transmit configuration to BLs |
123 | { |
124 | { |
124 | i2c_write_byte(pTxBuff[byte_counter-1]); // submit next byte |
125 | i2c_write_byte(pTxBuff[byte_counter-1]); // submit next byte |
125 | crc += pTxBuff[byte_counter-1]; // update crc |
126 | crc += pTxBuff[byte_counter-1]; // update crc |
126 | twi_state = 3; // stay in this state |
127 | twi_state = 3; // stay in this state |
127 | } |
128 | } |
128 | byte_counter++; // next byte |
129 | byte_counter++; // next byte |
129 | break; |
130 | break; |
130 | case 4: |
131 | case 4: |
131 | 132 | ||
132 | if(TWSR == 0x30) |
133 | if(TWSR == 0x30) |
133 | { |
134 | { |
134 | if(!missing_motor) missing_motor = motor + 1; |
135 | if(!missing_motor) missing_motor = motor + 1; |
135 | if((Motor[motor].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor].State++; // increment error counter and handle overflow |
136 | if((Motor[motor].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor].State++; // increment error counter and handle overflow |
136 | } |
137 | } |
137 | I2C_Stop(); |
138 | I2C_Stop(); |
138 | I2CTimeout = 10; |
139 | I2CTimeout = 10; |
139 | motor++; |
140 | motor++; |
140 | twi_state = 0; |
141 | twi_state = 0; |
141 | I2C_Start(); |
142 | I2C_Start(); |
142 | break; |
143 | break; |
143 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
144 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
144 | // Reading Data |
145 | // Reading Data |
145 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
146 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
146 | case 5: |
147 | case 5: |
147 | //Transmit 1st byte for reading |
148 | //Transmit 1st byte for reading |
148 | if(TWSR != 0x40) // Error? |
149 | if(TWSR != 0x40) // Error? |
149 | { |
150 | { |
150 | Motor[motorread].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit |
151 | Motor[motorread].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit |
151 | if(++motorread >= MAX_MOTORS) |
152 | if(++motorread >= MAX_MOTORS) |
152 | { |
153 | { |
153 | BLFlags &= ~BLFLAG_SEND_CONFIG; |
154 | BLFlags &= ~BLFLAG_SEND_CONFIG; |
154 | motorread = 0; |
155 | motorread = 0; |
155 | if(++motorread_temperature >= MAX_MOTORS) |
156 | if(++motorread_temperature >= MAX_MOTORS) |
156 | { |
157 | { |
157 | motorread_temperature = 0; |
158 | motorread_temperature = 0; |
158 | BLFlags &= ~BLFLAG_READ_VERSION; |
159 | BLFlags &= ~BLFLAG_READ_VERSION; |
159 | } |
160 | } |
160 | } |
161 | } |
161 | I2C_Stop(); |
162 | I2C_Stop(); |
162 | twi_state = 0; |
163 | twi_state = 0; |
163 | BLFlags |= BLFLAG_TX_COMPLETE; |
164 | BLFlags |= BLFLAG_TX_COMPLETE; |
164 | } |
165 | } |
165 | else |
166 | else |
166 | { |
167 | { |
167 | Motor[motorread].State |= MOTOR_STATE_PRESENT_MASK; // set present bit |
168 | Motor[motorread].State |= MOTOR_STATE_PRESENT_MASK; // set present bit |
168 | if(motorread == motorread_temperature) |
169 | if(motorread == motorread_temperature) |
169 | { |
170 | { |
170 | read_more = 1; |
171 | read_more = 1; |
171 | I2C_ReceiveByte(); |
172 | I2C_ReceiveByte(); |
172 | } |
173 | } |
173 | else |
174 | else |
174 | { |
175 | { |
175 | read_more = 0; |
176 | read_more = 0; |
176 | I2C_ReceiveLastByte(); |
177 | I2C_ReceiveLastByte(); |
177 | } |
178 | } |
178 | } |
179 | } |
179 | MissingMotor = missing_motor; |
180 | MissingMotor = missing_motor; |
180 | missing_motor = 0; |
181 | missing_motor = 0; |
181 | break; |
182 | break; |
182 | case 6: //Read 1st byte and transmit 2nd Byte |
183 | case 6: //Read 1st byte and transmit 2nd Byte |
183 | Motor[motorread].Current = TWDR; |
184 | Motor[motorread].Current = TWDR; |
184 | if(read_more) |
185 | if(read_more) |
185 | { |
186 | { |
186 | I2C_ReceiveByte() //ack |
187 | I2C_ReceiveByte() //ack |
187 | 188 | ||
188 | } |
189 | } |
189 | else |
190 | else |
190 | { |
191 | { |
191 | if(++motorread >= MAX_MOTORS) |
192 | if(++motorread >= MAX_MOTORS) |
192 | { |
193 | { |
193 | motorread = 0; // restart from beginning |
194 | motorread = 0; // restart from beginning |
194 | BLFlags &= ~BLFLAG_SEND_CONFIG; |
195 | BLFlags &= ~BLFLAG_SEND_CONFIG; |
195 | if(++motorread_temperature >= MAX_MOTORS) |
196 | if(++motorread_temperature >= MAX_MOTORS) |
196 | { |
197 | { |
197 | motorread_temperature = 0; |
198 | motorread_temperature = 0; |
198 | BLFlags &= ~BLFLAG_READ_VERSION; |
199 | BLFlags &= ~BLFLAG_READ_VERSION; |
199 | } |
200 | } |
200 | } |
201 | } |
201 | I2C_Stop(); |
202 | I2C_Stop(); |
202 | twi_state = 0; |
203 | twi_state = 0; |
203 | BLFlags |= BLFLAG_TX_COMPLETE; |
204 | BLFlags |= BLFLAG_TX_COMPLETE; |
204 | } |
205 | } |
205 | break; |
206 | break; |
206 | case 7: |
207 | case 7: |
207 | //Read 2nd byte and transmit 3rd Byte |
208 | //Read 2nd byte and transmit 3rd Byte |
208 | Motor[motorread].MaxPWM = TWDR; |
209 | Motor[motorread].MaxPWM = TWDR; |
209 | if(BLFlags & BLFLAG_READ_VERSION) |
210 | if(BLFlags & BLFLAG_READ_VERSION) |
210 | { |
211 | { |
211 | if(TWDR == 250) |
212 | if(TWDR == 250) |
212 | { |
213 | { |
213 | if(!MotorenEin) Motor[motorread].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK; |
214 | if(!MotorenEin) Motor[motorread].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK; |
214 | } |
215 | } |
215 | else |
216 | else |
216 | { |
217 | { |
217 | Motor[motorread].Version = 0; |
218 | Motor[motorread].Version = 0; |
218 | } |
219 | } |
219 | } |
220 | } |
220 | I2C_ReceiveLastByte(); //nack |
221 | I2C_ReceiveLastByte(); //nack |
221 | break; |
222 | break; |
222 | case 8: // read next |
223 | case 8: // read next |
223 | Motor[motorread].Temperature = TWDR; |
224 | Motor[motorread].Temperature = TWDR; |
224 | if(++motorread >= MAX_MOTORS) |
225 | if(++motorread >= MAX_MOTORS) |
225 | { |
226 | { |
226 | motorread = 0; // restart reading of first motor |
227 | motorread = 0; // restart reading of first motor |
227 | BLFlags &= ~BLFLAG_SEND_CONFIG; |
228 | BLFlags &= ~BLFLAG_SEND_CONFIG; |
228 | if(++motorread_temperature >= MAX_MOTORS) |
229 | if(++motorread_temperature >= MAX_MOTORS) |
229 | { |
230 | { |
230 | motorread_temperature = 0; |
231 | motorread_temperature = 0; |
231 | BLFlags &= ~BLFLAG_READ_VERSION; |
232 | BLFlags &= ~BLFLAG_READ_VERSION; |
232 | } |
233 | } |
233 | } |
234 | } |
234 | I2C_Stop(); |
235 | I2C_Stop(); |
235 | BLFlags |= BLFLAG_TX_COMPLETE; |
236 | BLFlags |= BLFLAG_TX_COMPLETE; |
236 | twi_state = 0; |
237 | twi_state = 0; |
237 | break; |
238 | break; |
238 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
239 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
239 | // writing Gyro-Offset |
240 | // writing Gyro-Offset |
240 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
241 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
241 | case 18: |
242 | case 18: |
242 | i2c_write_byte(0x98); // Address of the DAC |
243 | i2c_write_byte(0x98); // Address of the DAC |
243 | break; |
244 | break; |
244 | case 19: |
245 | case 19: |
245 | i2c_write_byte(0x10); // Update Channel A |
246 | i2c_write_byte(0x10); // Update Channel A |
246 | break; |
247 | break; |
247 | case 20: |
248 | case 20: |
248 | i2c_write_byte(AnalogOffsetNick); // Value |
249 | i2c_write_byte(AnalogOffsetNick); // Value |
249 | break; |
250 | break; |
250 | case 21: |
251 | case 21: |
251 | i2c_write_byte(0x80); // Value |
252 | i2c_write_byte(0x80); // Value |
252 | break; |
253 | break; |
253 | case 22: |
254 | case 22: |
254 | I2C_Stop(); |
255 | I2C_Stop(); |
255 | I2CTimeout = 10; |
256 | I2CTimeout = 10; |
256 | I2C_Start(); |
257 | I2C_Start(); |
257 | break; |
258 | break; |
258 | case 23: |
259 | case 23: |
259 | i2c_write_byte(0x98); // Address of the DAC |
260 | i2c_write_byte(0x98); // Address of the DAC |
260 | break; |
261 | break; |
261 | case 24: |
262 | case 24: |
262 | i2c_write_byte(0x12); // Update Channel B |
263 | i2c_write_byte(0x12); // Update Channel B |
263 | break; |
264 | break; |
264 | case 25: |
265 | case 25: |
265 | i2c_write_byte(AnalogOffsetRoll); // Value |
266 | i2c_write_byte(AnalogOffsetRoll); // Value |
266 | break; |
267 | break; |
267 | case 26: |
268 | case 26: |
268 | i2c_write_byte(0x80); // Value |
269 | i2c_write_byte(0x80); // Value |
269 | break; |
270 | break; |
270 | case 27: |
271 | case 27: |
271 | I2C_Stop(); |
272 | I2C_Stop(); |
272 | I2CTimeout = 10; |
273 | I2CTimeout = 10; |
273 | I2C_Start(); |
274 | I2C_Start(); |
274 | break; |
275 | break; |
275 | case 28: |
276 | case 28: |
276 | i2c_write_byte(0x98); // Address of the DAC |
277 | i2c_write_byte(0x98); // Address of the DAC |
277 | break; |
278 | break; |
278 | case 29: |
279 | case 29: |
279 | i2c_write_byte(0x14); // Update Channel C |
280 | i2c_write_byte(0x14); // Update Channel C |
280 | break; |
281 | break; |
281 | case 30: |
282 | case 30: |
282 | i2c_write_byte(AnalogOffsetGier); // Value |
283 | i2c_write_byte(AnalogOffsetGier); // Value |
283 | break; |
284 | break; |
284 | case 31: |
285 | case 31: |
285 | i2c_write_byte(0x80); // Value |
286 | i2c_write_byte(0x80); // Value |
286 | break; |
287 | break; |
287 | case 32: |
288 | case 32: |
288 | I2C_Stop(); |
289 | I2C_Stop(); |
289 | I2CTimeout = 10; |
290 | I2CTimeout = 10; |
290 | twi_state = 0; |
291 | twi_state = 0; |
291 | break; |
292 | break; |
292 | default: twi_state = 0; |
293 | default: twi_state = 0; |
293 | break; |
294 | break; |
294 | } |
295 | } |
295 | TWCR |= 0x80; |
296 | TWCR |= 0x80; |
296 | J4Low; |
297 | J4Low; |
297 | } |
298 | } |
298 | 299 | ||
299 | void I2C_SendBLConfig(void) |
300 | void I2C_SendBLConfig(void) |
300 | { |
301 | { |
301 | unsigned char i; |
302 | unsigned char i; |
302 | while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer |
303 | while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer |
303 | BLFlags |= BLFLAG_SEND_CONFIG; // enable sending of BL config |
304 | BLFlags |= BLFLAG_SEND_CONFIG; // enable sending of BL config |
304 | for(i = 0; i < MAX_MOTORS; i++) |
305 | for(i = 0; i < MAX_MOTORS; i++) |
305 | { |
306 | { |
306 | Motor[i].SetPoint = 0; |
307 | Motor[i].SetPoint = 0; |
307 | Motor[i].SetPointLowerBits = 0; |
308 | Motor[i].SetPointLowerBits = 0; |
308 | } |
309 | } |
309 | motorread = 0; |
310 | motorread = 0; |
310 | // needs at least MAX_MOTORS loops of 2 ms (12*2ms = 24ms) |
311 | // needs at least MAX_MOTORS loops of 2 ms (12*2ms = 24ms) |
311 | do |
312 | do |
312 | { |
313 | { |
313 | twi_state = 0; |
314 | twi_state = 0; |
314 | I2C_Start(); // start an i2c transmission |
315 | I2C_Start(); // start an i2c transmission |
315 | while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer |
316 | while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer |
316 | }while(BLFlags & BLFLAG_SEND_CONFIG); // repeat until the BL config has been send to all motors |
317 | }while(BLFlags & BLFLAG_SEND_CONFIG); // repeat until the BL config has been send to all motors |
317 | } |
318 | } |
318 | 319 | ||
319 | 320 | ||
320 | 321 | ||
321 | 322 |