Rev 1868 | Rev 1870 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1868 | Rev 1869 | ||
---|---|---|---|
Line 214... | Line 214... | ||
214 | LCD_Clear(); |
214 | LCD_Clear(); |
Line 215... | Line 215... | ||
215 | 215 | ||
Line 216... | Line 216... | ||
216 | I2CTimeout = 5000; |
216 | I2CTimeout = 5000; |
217 | 217 | ||
218 | while (1) { |
218 | while (1) { |
Line 219... | Line -... | ||
219 | if (runFlightControl && analogDataReady) { // control interval |
- | |
220 | runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
- | |
221 | - | ||
222 | //J4HIGH; |
- | |
223 | flight_control(); |
- | |
224 | //J4LOW; |
- | |
225 | - | ||
226 | /* |
- | |
227 | * If the motors are running (MKFlags & MKFLAG_MOTOR_RUN in flight.c), transmit |
- | |
228 | * the throttle vector just computed. Otherwise, if motor test is engaged, transmit |
- | |
229 | * the test throttle vector. If no testing, stop all motors. |
- | |
230 | */ |
- | |
231 | // Obsoleted. |
- | |
232 | // transmitMotorThrottleData(); |
- | |
233 | - | ||
234 | RED_OFF; |
- | |
235 | - | ||
236 | /* |
219 | if (runFlightControl) { // control interval |
237 | Does not belong here. Instead, external control should be ignored in |
- | |
238 | controlMixer if there was no new data from there for some time. |
- | |
239 | if(externalControlActive) externalControlActive--; |
- | |
240 | else { |
- | |
241 | externalControl.config = 0; |
- | |
242 | externalStickPitch = 0; |
- | |
243 | externalStickRoll = 0; |
- | |
244 | externalStickYaw = 0; |
- | |
245 | } |
- | |
246 | */ |
- | |
247 | - | ||
248 | /* |
- | |
249 | Does not belong here. |
- | |
250 | if(RC_Quality) RC_Quality--; |
- | |
251 | */ |
- | |
252 | - | ||
253 | /* Does not belong here. Well since we are not supporting navi right now anyway, leave out. |
220 | runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
254 | #ifdef USE_NAVICTRL |
- | |
255 | if(NCDataOkay) { |
- | |
256 | if(--NCDataOkay == 0) // no data from NC |
- | |
257 | { // set gps control sticks neutral |
- | |
258 | GPSStickPitch = 0; |
- | |
259 | GPSStickRoll = 0; |
- | |
260 | NCSerialDataOkay = 0; |
- | |
261 | } |
- | |
262 | } |
- | |
263 | #endif |
- | |
264 | */ |
- | |
265 | if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing ot timeout |
- | |
266 | RED_ON; |
- | |
267 | if (!I2CTimeout) { |
- | |
268 | I2C_Reset(); |
- | |
269 | I2CTimeout = 5; |
- | |
270 | } |
- | |
Line -... | Line 221... | ||
- | 221 | ||
- | 222 | DebugOut.Digital[0] &= ~DEBUG_MAINLOOP_TIMER; |
|
- | 223 | DebugOut.Digital[1] &= ~DEBUG_MAINLOOP_TIMER; |
|
- | 224 | ||
- | 225 | if (analogDataReady) { |
|
- | 226 | J4HIGH; |
|
271 | } else { |
227 | flight_control(); |
272 | RED_OFF; |
228 | J4LOW; |
273 | } |
229 | |
274 | 230 | /* |
|
- | 231 | * If the motors are running (MKFlags & MKFLAG_MOTOR_RUN in flight.c), transmit |
|
- | 232 | * the throttle vector just computed. Otherwise, if motor test is engaged, transmit |
|
Line -... | Line 233... | ||
- | 233 | * the test throttle vector. If no testing, stop all motors. |
|
- | 234 | */ |
|
- | 235 | // Obsoleted. |
|
- | 236 | // transmitMotorThrottleData(); |
|
- | 237 | ||
- | 238 | RED_OFF; |
|
- | 239 | ||
- | 240 | /* |
|
- | 241 | Does not belong here. Instead, external control should be ignored in |
|
- | 242 | controlMixer if there was no new data from there for some time. |
|
- | 243 | if(externalControlActive) externalControlActive--; |
|
- | 244 | else { |
|
- | 245 | externalControl.config = 0; |
|
- | 246 | externalStickPitch = 0; |
|
- | 247 | externalStickRoll = 0; |
|
- | 248 | externalStickYaw = 0; |
|
- | 249 | } |
|
- | 250 | */ |
|
- | 251 | ||
- | 252 | /* |
|
- | 253 | Does not belong here. |
|
- | 254 | if(RC_Quality) RC_Quality--; |
|
- | 255 | */ |
|
- | 256 | ||
- | 257 | /* Does not belong here. Well since we are not supporting navi right now anyway, leave out. |
|
- | 258 | #ifdef USE_NAVICTRL |
|
- | 259 | if(NCDataOkay) { |
|
- | 260 | if(--NCDataOkay == 0) // no data from NC |
|
- | 261 | { // set gps control sticks neutral |
|
- | 262 | GPSStickPitch = 0; |
|
- | 263 | GPSStickRoll = 0; |
|
- | 264 | NCSerialDataOkay = 0; |
|
- | 265 | } |
|
- | 266 | } |
|
275 | // Allow Serial Data Transmit if motors must not updated or motors are not running |
267 | #endif |
- | 268 | */ |
|
- | 269 | if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing ot timeout |
|
- | 270 | RED_ON; |
|
- | 271 | if (!I2CTimeout) { |
|
- | 272 | I2C_Reset(); |
|
Line 276... | Line -... | ||
276 | if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) { |
- | |
277 | usart0_TransmitTxData(); |
- | |
278 | } |
273 | I2CTimeout = 5; |
279 | 274 | } |
|
280 | usart0_ProcessRxData(); |
275 | } else { |
281 | 276 | RED_OFF; |
|
- | 277 | } |
|
282 | if (CheckDelay(timer)) { |
278 | |
Line -... | Line 279... | ||
- | 279 | // Allow Serial Data Transmit if motors must not updated or motors are not running |
|
- | 280 | if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) { |
|
- | 281 | usart0_TransmitTxData(); |
|
- | 282 | } else { |
|
- | 283 | DebugOut.Digital[1] |= DEBUG_MAINLOOP_TIMER; |
|
- | 284 | } |
|
- | 285 | ||
- | 286 | usart0_ProcessRxData(); |
|
- | 287 | ||
- | 288 | if (CheckDelay(timer)) { |
|
283 | if (UBat <= UBAT_AT_5V) { |
289 | if (UBat <= UBAT_AT_5V) { |
284 | // Do nothing. The voltage on the input side of the regulator is <5V; |
290 | // Do nothing. The voltage on the input side of the regulator is <5V; |
285 | // we must be running off USB power. Keep it quiet. |
291 | // we must be running off USB power. Keep it quiet. |
286 | } else if (UBat < staticParams.LowVoltageWarning) { |
292 | } else if (UBat < staticParams.LowVoltageWarning) { |
287 | beepBatteryAlarm(); |
293 | beepBatteryAlarm(); |
- | 294 | } |
|
- | 295 | ||
288 | } |
296 | #ifdef USE_NAVICTRL |
289 | - | ||
290 | #ifdef USE_NAVICTRL |
- | |
Line 291... | Line 297... | ||
291 | SPI_StartTransmitPacket(); |
297 | SPI_StartTransmitPacket(); |
292 | SendSPI = 4; |
298 | SendSPI = 4; |
293 | #endif |
299 | #endif |
294 | timer = SetDelay(20); // every 20 ms |
300 | timer = SetDelay(20); // every 20 ms |
295 | } |
301 | } |
296 | output_update(); |
302 | output_update(); |
297 | } |
303 | } |
298 | 304 | ||
299 | #ifdef USE_NAVICTRL |
305 | #ifdef USE_NAVICTRL |
- | 306 | if(!SendSPI) { |
|
300 | if(!SendSPI) { |
307 | // SendSPI is decremented in timer0.c with a rate of 9.765 kHz. |
301 | // SendSPI is decremented in timer0.c with a rate of 9.765 kHz. |
308 | // within the SPI_TransmitByte() routine the value is set to 4. |
302 | // within the SPI_TransmitByte() routine the value is set to 4. |
309 | // I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz, |