Rev 659 | Rev 688 | 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 | } |