175,7 → 175,6 |
DateTime_t GPSDateTime = {0,0,0,0,0,0,0, INVALID}; |
|
|
|
//------------------------------------------------------------------------------------ |
// functions |
|
276,32 → 275,50 |
/********************************************************/ |
void Update_GPSData (void) |
{ |
if ((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA) && (UbxVelNed.Status == NEWDATA)) |
{ |
LED_RED_TOGGLE; |
// update GPS data only if the status is INVALID or PROCESSED |
if(GPSData.Status != NEWDATA) // if last data were processed |
{ // wait for new data at all neccesary ubx messages |
GPSData.Status = INVALID; |
// NAV SOL |
GPSData.Flags = UbxSol.Flags; |
GPSData.NumOfSats = UbxSol.numSV; |
GPSData.SatFix = UbxSol.GPSfix; |
GPSData.Position_Accuracy = UbxSol.PAcc; |
GPSData.Speed_Accuracy = UbxSol.SAcc; |
SetGPSTime(&SystemTime); // update system time |
// NAV POSLLH |
GPSData.Longitude = UbxPosLlh.LON; |
GPSData.Latitude = UbxPosLlh.LAT; |
GPSData.Altitude = UbxPosLlh.HMSL; |
// NAV VELNED |
GPSData.Speed_East = UbxVelNed.VEL_E; |
GPSData.Speed_North = UbxVelNed.VEL_N; |
GPSData.Speed_Top = -UbxVelNed.VEL_D; |
GPSData.Speed_Ground = UbxVelNed.GSpeed; |
|
GPSData.Status = NEWDATA; // new data available |
} |
static u32 Ubx_Timeout = 0; |
static u8 Msg_Count = 0; |
|
// the timeout is used to detect the delay between two message sets |
// and is used for synchronisation so that always a set is collected |
// that belongs together |
// _______NAVSOL|POSLLH|VELNED|___________________NAVSOL|POSLLH|VELNED|_____________ |
// | 8ms | 8ms | 184 ms | | | |
// msg_count: 0 1 2 0 1 2 |
|
if(CheckDelay(Ubx_Timeout)) Msg_Count = 0; |
else Msg_Count++; |
Ubx_Timeout = SetDelay(100); // reset ubx msg timeout |
|
// if a new set of ubx messages was collected |
if((Msg_Count >= 2)) |
{ // if set is complete |
if((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA) && (UbxVelNed.Status == NEWDATA)) |
{ |
LED_RED_TOGGLE; |
// update GPS data only if the status is INVALID or PROCESSED and the last ubx message was received within less than 100 ms |
if((GPSData.Status != NEWDATA)) // if last data were processed |
{ // wait for new data at all neccesary ubx messages |
GPSData.Status = INVALID; |
// NAV SOL |
GPSData.Flags = UbxSol.Flags; |
GPSData.NumOfSats = UbxSol.numSV; |
GPSData.SatFix = UbxSol.GPSfix; |
GPSData.Position_Accuracy = UbxSol.PAcc; |
GPSData.Speed_Accuracy = UbxSol.SAcc; |
SetGPSTime(&SystemTime); // update system time |
// NAV POSLLH |
GPSData.Longitude = UbxPosLlh.LON; |
GPSData.Latitude = UbxPosLlh.LAT; |
GPSData.Altitude = UbxPosLlh.HMSL; |
// NAV VELNED |
GPSData.Speed_East = UbxVelNed.VEL_E; |
GPSData.Speed_North = UbxVelNed.VEL_N; |
GPSData.Speed_Top = -UbxVelNed.VEL_D; |
GPSData.Speed_Ground = UbxVelNed.GSpeed; |
|
GPSData.Status = NEWDATA; // new data available |
} // EOF if(GPSData.Status != NEWDATA) |
} // EOF all ubx messages received |
// set state to collect new data |
UbxSol.Status = PROCESSED; // ready for new data |
UbxPosLlh.Status = PROCESSED; // ready for new data |