Rev 1644 | Rev 1652 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1644 | Rev 1648 | ||
|---|---|---|---|
| Line 165... | Line 165... | ||
| 165 | 165 | ||
| 166 | 166 | ||
| 167 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
167 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| - | 168 | // + Check connected BL-Ctrls |
|
| - | 169 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 170 | // Check connected BL-Ctrls |
|
| - | 171 | BLFlags |= BLFLAG_READ_VERSION; |
|
| 168 | // + Check connected BL-Ctrls |
172 | UpdateMotor = 0; |
| - | 173 | SendMotorData(); |
|
| 169 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
174 | while(!UpdateMotor); |
| 170 | motorread = 0; UpdateMotor = 0; SendMotorData(); while(!UpdateMotor); motorread = 0; // read the first I2C-Data |
175 | motorread = 0; |
| 171 | printf("\n\rFound BL-Ctrl: "); |
- | |
| 172 | timer = SetDelay(4000); |
176 | printf("\n\rFound BL-Ctrl: "); |
| 173 | 177 | timer = SetDelay(4000); |
|
| 174 | for(i=0; i < MAX_MOTORS; i++) |
178 | for(i=0; i < MAX_MOTORS; i++) |
| 175 | { |
179 | { |
| 176 | UpdateMotor = 0; |
180 | UpdateMotor = 0; |
| 177 | SendMotorData(); |
181 | SendMotorData(); |
| 178 | while(!UpdateMotor); |
182 | while(!UpdateMotor); |
| 179 | if(Mixer.Motor[i][0] > 0) // wait max 4 sec for the BL-Ctrls to wake up |
183 | if(Mixer.Motor[i][0] > 0) // wait max 4 sec for the BL-Ctrls to wake up |
| - | 184 | { |
|
| - | 185 | while(!CheckDelay(timer) && !(Motor[i].State & MOTOR_STATE_PRESENT_MASK) ) |
|
| - | 186 | { |
|
| - | 187 | UpdateMotor = 0; |
|
| - | 188 | SendMotorData(); |
|
| - | 189 | while(!UpdateMotor); |
|
| - | 190 | } |
|
| - | 191 | } |
|
| - | 192 | if(Motor[i].State & MOTOR_STATE_PRESENT_MASK) |
|
| - | 193 | { |
|
| - | 194 | printf("%d",i+1); |
|
| 180 | { |
195 | if(Motor[i].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) printf("(new) "); |
| 181 | while(!CheckDelay(timer) && !(Motor[i].State & MOTOR_STATE_PRESENT_MASK) ) {UpdateMotor = 0; SendMotorData(); while(!UpdateMotor);}; |
- | |
| 182 | } |
- | |
| 183 | if(Motor[i].State & MOTOR_STATE_PRESENT_MASK) |
- | |
| 184 | { |
- | |
| 185 | printf("%d",i+1); |
- | |
| 186 | if(Motor[i].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) printf("(new) "); else printf(" "); |
196 | else printf(" "); |
| 187 | } |
197 | } |
| 188 | } |
198 | } |
| 189 | for(i=0; i < MAX_MOTORS; i++) |
199 | for(i=0; i < MAX_MOTORS; i++) |
| 190 | { |
200 | { |
| Line 194... | Line 204... | ||
| 194 | ServoActive = 1; // just in case the FC would be used as camera-stabilizer |
204 | ServoActive = 1; // just in case the FC would be used as camera-stabilizer |
| 195 | } |
205 | } |
| 196 | Motor[i].State &= ~MOTOR_STATE_ERROR_MASK; // clear error counter |
206 | Motor[i].State &= ~MOTOR_STATE_ERROR_MASK; // clear error counter |
| 197 | } |
207 | } |
| 198 | printf("\n\r==================================="); |
208 | printf("\n\r==================================="); |
| 199 | SendMotorData(); |
- | |
| - | 209 | ||
| 200 | TransmitBlConfig = 1; |
210 | I2C_SendBLConfig(); |
| 201 | motorread = 0; |
211 | |
| 202 | //if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) |
212 | //if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) |
| 203 | { |
213 | { |
| 204 | printf("\n\rCalibrating pressure sensor.."); |
214 | printf("\n\rCalibrating pressure sensor.."); |
| 205 | timer = SetDelay(1000); |
215 | timer = SetDelay(1000); |
| 206 | SucheLuftruckOffset(); |
216 | SucheLuftruckOffset(); |
| Line 239... | Line 249... | ||
| 239 | 249 | ||
| 240 | printf("\n\r===================================\n\r"); |
250 | printf("\n\r===================================\n\r"); |
| 241 | //SpektrumBinding(); |
251 | //SpektrumBinding(); |
| 242 | timer = SetDelay(2000); |
252 | timer = SetDelay(2000); |
| 243 | timerPolling = SetDelay(250); |
253 | timerPolling = SetDelay(250); |
| 244 | 254 | ||
| 245 | Debug(ANSI_CLEAR "FC-Start!\n\rFlugzeit: %d min", FlugMinutenGesamt); // Note: this won't waste flash memory, if #DEBUG is not active |
255 | Debug(ANSI_CLEAR "FC-Start!\n\rFlugzeit: %d min", FlugMinutenGesamt); // Note: this won't waste flash memory, if #DEBUG is not active |
| 246 | 256 | ||
| 247 | while (1) |
257 | while (1) |
| 248 | { |
258 | { |
| 249 | if(CheckDelay(timerPolling)) |
259 | if(CheckDelay(timerPolling)) |
| 250 | { |
260 | { |