Rev 659 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 659 | Rev 663 | ||
|---|---|---|---|
| Line 99... | Line 99... | ||
| 99 | volatile u32 PollingTimeout = 10000; |
99 | volatile u32 PollingTimeout = 10000; |
| 100 | Param_t Parameter; |
100 | Param_t Parameter; |
| 101 | volatile FC_t FC; |
101 | volatile FC_t FC; |
| 102 | volatile u32 SPIWatchDog = 15000; // stop Navigation if this goes to zero |
102 | volatile u32 SPIWatchDog = 15000; // stop Navigation if this goes to zero |
| 103 | volatile u32 SD_WatchDog = 15000; // stop Logging if this goes to zero |
103 | volatile u32 SD_WatchDog = 15000; // stop Logging if this goes to zero |
| 104 | u32 CountGpsProcessedIn5Sec = 50,CountNewGpsDataIn5Sec = 25, FreqGpsProcessedIn5Sec = 0, FreqNewGpsData = 0; |
104 | u32 CountGpsProcessedIn5Sec = 50,CountNewGpsDataIn5Sec = 25, FreqGpsNavProcessed = 0, FreqNewGpsData = 0; |
| 105 | u8 NewWPL_Name = 0; |
105 | u8 NewWPL_Name = 0; |
| 106 | u32 MaxWP_Radius_in_m = 0; |
106 | u32 MaxWP_Radius_in_m = 0; |
| 107 | s8 ErrorMSG[25]; |
107 | s8 ErrorMSG[25]; |
| 108 | u32 TimeSinceMotorStart = 0; |
108 | u32 TimeSinceMotorStart = 0; |
| Line 391... | Line 391... | ||
| 391 | { |
391 | { |
| 392 | LED_RED_ON; |
392 | LED_RED_ON; |
| 393 | sprintf(ErrorMSG,"No GPS Fix "); |
393 | sprintf(ErrorMSG,"No GPS Fix "); |
| 394 | newErrorCode = 30; |
394 | newErrorCode = 30; |
| 395 | } |
395 | } |
| 396 | else if((FreqNewGpsData < 28 || FreqNewGpsData > 60) && TimeSinceMotorStart > 5) |
396 | else if((FreqNewGpsData <= 35 || FreqNewGpsData > 60) && TimeSinceMotorStart > 15) |
| 397 | { |
397 | { |
| 398 | LED_RED_ON; |
398 | LED_RED_ON; |
| 399 | sprintf(ErrorMSG,"GPS Update rate "); |
399 | sprintf(ErrorMSG,"GPS Update rate "); |
| 400 | newErrorCode = 38; |
400 | newErrorCode = 38; |
| 401 | } |
401 | } |
| Line 472... | Line 472... | ||
| 472 | UART1_TransmitTxData(); // PC send answer |
472 | UART1_TransmitTxData(); // PC send answer |
| 473 | UART2_TransmitTxData(); // FC send answer |
473 | UART2_TransmitTxData(); // FC send answer |
| Line 474... | Line 474... | ||
| 474 | 474 | ||
| Line -... | Line 475... | ||
| - | 475 | if(!(FC.StatusFlags & FC_STATUS_MOTOR_RUN)) TimeSinceMotorStart = 0; |
|
| - | 476 | ||
| - | 477 | if(FCCalibActive) |
|
| - | 478 | { |
|
| - | 479 | count5sec = 0; |
|
| - | 480 | FreqNewGpsData = 50; |
|
| - | 481 | CountNewGpsDataIn5Sec = 25; |
|
| - | 482 | CountGpsProcessedIn5Sec = 0; |
|
| - | 483 | TimerCheckError = SetDelay(1000); |
|
| 475 | if(!(FC.StatusFlags & FC_STATUS_MOTOR_RUN)) TimeSinceMotorStart = 0; |
484 | } |
| 476 | 485 | ||
| 477 | // ---------------- Error Check Timing ---------------------------- |
486 | // ---------------- Error Check Timing ---------------------------- |
| 478 | if(CheckDelay(TimerCheckError) || (FC.StatusFlags & FC_STATUS_START && !(oldFcFlags & FC_STATUS_START))) // Timer or FY wants to start |
487 | if(CheckDelay(TimerCheckError) || (FC.StatusFlags & FC_STATUS_START && !(oldFcFlags & FC_STATUS_START))) // Timer or FY wants to start |
| 479 | { |
488 | { |
| 480 | if(CheckDelay(TimerCheckError)) |
489 | if(CheckDelay(TimerCheckError)) |
| 481 | { |
490 | { |
| 482 | TimerCheckError = SetDelay(1000); |
491 | TimerCheckError = SetDelay(1000); |
| 483 | if(CompassValueErrorCount) CompassValueErrorCount--; |
492 | if(CompassValueErrorCount) CompassValueErrorCount--; |
| - | 493 | if(++count5sec == 5) |
|
| - | 494 | { |
|
| - | 495 | FreqGpsNavProcessed = CountGpsProcessedIn5Sec * 2; //400 = 40Hz |
|
| 484 | if(++count5sec == 5) |
496 | FreqNewGpsData = CountNewGpsDataIn5Sec; // 50 = 5,0Hz |
| 485 | { |
- | |
| 486 | count5sec = 0; |
- | |
| 487 | FreqGpsProcessedIn5Sec = CountGpsProcessedIn5Sec * 2; |
497 | |
| 488 | FreqNewGpsData = CountNewGpsDataIn5Sec; |
498 | count5sec = 0; |
| - | 499 | CountGpsProcessedIn5Sec = 0; |
|
| 489 | CountGpsProcessedIn5Sec = 0; |
500 | CountNewGpsDataIn5Sec = FreqNewGpsData / 2; |
| 490 | CountNewGpsDataIn5Sec = FreqNewGpsData / 2; |
501 | ConfirmGpsUpdateRate(FreqNewGpsData); |
| 491 | } |
502 | } |
| 492 | } |
503 | } |