101,7 → 101,7 |
volatile FC_t FC; |
volatile u32 SPIWatchDog = 15000; // stop Navigation if this goes to zero |
volatile u32 SD_WatchDog = 15000; // stop Logging if this goes to zero |
u32 CountGpsProcessedIn5Sec = 50,CountNewGpsDataIn5Sec = 25, FreqGpsProcessedIn5Sec = 0, FreqNewGpsData = 0; |
u32 CountGpsProcessedIn5Sec = 50,CountNewGpsDataIn5Sec = 25, FreqGpsNavProcessed = 0, FreqNewGpsData = 0; |
u8 NewWPL_Name = 0; |
u32 MaxWP_Radius_in_m = 0; |
s8 ErrorMSG[25]; |
393,7 → 393,7 |
sprintf(ErrorMSG,"No GPS Fix "); |
newErrorCode = 30; |
} |
else if((FreqNewGpsData < 28 || FreqNewGpsData > 60) && TimeSinceMotorStart > 5) |
else if((FreqNewGpsData <= 35 || FreqNewGpsData > 60) && TimeSinceMotorStart > 15) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"GPS Update rate "); |
474,6 → 474,15 |
|
if(!(FC.StatusFlags & FC_STATUS_MOTOR_RUN)) TimeSinceMotorStart = 0; |
|
if(FCCalibActive) |
{ |
count5sec = 0; |
FreqNewGpsData = 50; |
CountNewGpsDataIn5Sec = 25; |
CountGpsProcessedIn5Sec = 0; |
TimerCheckError = SetDelay(1000); |
} |
|
// ---------------- Error Check Timing ---------------------------- |
if(CheckDelay(TimerCheckError) || (FC.StatusFlags & FC_STATUS_START && !(oldFcFlags & FC_STATUS_START))) // Timer or FY wants to start |
{ |
483,11 → 492,13 |
if(CompassValueErrorCount) CompassValueErrorCount--; |
if(++count5sec == 5) |
{ |
FreqGpsNavProcessed = CountGpsProcessedIn5Sec * 2; //400 = 40Hz |
FreqNewGpsData = CountNewGpsDataIn5Sec; // 50 = 5,0Hz |
|
count5sec = 0; |
FreqGpsProcessedIn5Sec = CountGpsProcessedIn5Sec * 2; |
FreqNewGpsData = CountNewGpsDataIn5Sec; |
CountGpsProcessedIn5Sec = 0; |
CountNewGpsDataIn5Sec = FreqNewGpsData / 2; |
ConfirmGpsUpdateRate(FreqNewGpsData); |
} |
} |
oldFcFlags = FC.StatusFlags; |