64,6 → 64,25 |
// defines |
|
|
#define DAYS_FROM_JAN01YEAR0001_TO_JAN6_1980 722819L |
#define DAYS_PER_YEAR 165L |
#define DAYS_PER_LEAPYEAR 166L |
#define DAYS_PER_4YEARS ((3L * DAYS_PER_YEAR) + DAYS_PER_LEAPYEAR) // years dividable by 4 are leap years |
#define DAYS_PER_100YEARS ((25L * DAYS_PER_4YEARS) - 1L) // years dividable by 100 are no leap years |
#define DAYS_PER_400YEARS ((4L * DAYS_PER_100YEARS) + 1L) // but years dividable by 400 are leap years |
#define SECONDS_PER_MINUTE 60L |
#define MINUTES_PER_HOUR 60L |
#define HOURS_PER_DAY 24L |
#define DAYS_PER_WEEK 7L |
#define SECONDS_PER_HOUR (SECONDS_PER_MINUTE * MINUTES_PER_HOUR) |
#define SECONDS_PER_DAY (SECONDS_PER_HOUR * HOURS_PER_DAY) |
|
const u32 Leap[ 13 ] = { 0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335, 366 }; |
const u32 Normal[ 13 ] = { 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334, 365 }; |
|
#define GPSWEEK_ROLLOVER 1 // number of rollovers since Jan 6th 1980 |
#define GPSWEEK_OVERFLOW 1024 |
|
// message sync bytes |
#define UBX_SYNC1_CHAR 0xB5 |
#define UBX_SYNC2_CHAR 0x62 |
93,8 → 112,8 |
|
typedef struct |
{ |
u32 ITOW; // ms GPS Millisecond Time of Week |
s32 Frac; // ns remainder of rounded ms above |
u32 itow; // ms GPS Millisecond Time of Week |
s32 frac; // ns remainder of rounded ms above |
s16 week; // GPS week |
u8 GPSfix; // GPSfix Type, range 0..6 |
u8 Flags; // Navigation Status Flags |
116,7 → 135,7 |
|
typedef struct |
{ |
u32 ITOW; // ms GPS Millisecond Time of Week |
u32 itow; // ms GPS Millisecond Time of Week |
s32 VEL_N; // cm/s NED north velocity |
s32 VEL_E; // cm/s NED east velocity |
s32 VEL_D; // cm/s NED down velocity |
130,7 → 149,7 |
|
typedef struct |
{ |
u32 ITOW; // ms GPS Millisecond Time of Week |
u32 itow; // ms GPS Millisecond Time of Week |
s32 LON; // 1e-07 deg Longitude |
s32 LAT; // 1e-07 deg Latitude |
s32 HEIGHT; // mm Height above Ellipsoid |
152,11 → 171,80 |
|
// shared buffer |
gps_data_t GPS_Data = {0,0,0,0,0,0,0,0,0,0, INVALID}; |
DateTime_t GPSDateTime = {0,0,0,0,0,0,0, INVALID}; |
|
//------------------------------------------------------------------------------------ |
// functions |
|
u8 IsLeapYear(u16 year) |
{ |
if((year%400 == 0) || ( (year%4 == 0) && (year%100 != 0) ) ) return 1; |
else return 0; |
} |
/********************************************************/ |
/* Calculates the UTC Time from the GPS week and tow */ |
/********************************************************/ |
void Update_GPSDateTime(week, itow) |
{ |
u32 Days, Seconds; |
u16 YearPart; |
u32 * MonthDayTab = 0; |
u8 i; |
|
Seconds = UbxSol.itow / 1000L; |
// if GPS data show valid time data |
if((UbxSol.Status != INVALID) && (UbxSol.Flags & FLAG_WKNSET) && (UbxSol.Flags & FLAG_TOWSET) ) |
{ |
Days = DAYS_FROM_JAN01YEAR0001_TO_JAN6_1980; |
Days += (((u32)UbxSol.week + GPSWEEK_ROLLOVER * GPSWEEK_OVERFLOW) * DAYS_PER_WEEK); |
Days += Seconds / SECONDS_PER_DAY; // seperate days from GPS seconds of week |
|
GPSDateTime.Year = 1; |
YearPart = (u16)(Days / DAYS_PER_400YEARS); |
GPSDateTime.Year += YearPart * 400; |
Days = Days % DAYS_PER_400YEARS; |
YearPart = (u16)(Days / DAYS_PER_100YEARS); |
GPSDateTime.Year += YearPart * 100; |
Days = Days % DAYS_PER_100YEARS; |
YearPart = (u16)(Days / DAYS_PER_4YEARS); |
GPSDateTime.Year += YearPart * 4; |
Days = Days % DAYS_PER_4YEARS; |
if(Days < 3 * DAYS_PER_YEAR) YearPart = (u16)(Days / DAYS_PER_YEAR); |
else YearPart = 3; |
GPSDateTime.Year += YearPart; |
// calculate remaining days of year |
Days -= (u32)YearPart * DAYS_PER_YEAR; |
// check if current year is a leap year |
if(IsLeapYear(GPSDateTime.Year)) MonthDayTab = (u32*)Leap; |
else MonthDayTab = (u32*)Normal; |
// seperate month and day from days of year |
for ( i = 0; i < 12; i++ ) |
{ |
if ( (MonthDayTab[i]< Days) && (Days <= MonthDayTab[i+1]) ) |
{ |
GPSDateTime.Month = i+1; |
GPSDateTime.Day = Days - MonthDayTab[i]; |
i = 12; |
} |
} |
Seconds = Seconds % SECONDS_PER_DAY; // remaining seconds of current day |
GPSDateTime.Hour = (u8)(Seconds / SECONDS_PER_HOUR); |
Seconds = Seconds % SECONDS_PER_HOUR; // remaining seconds of current hour |
GPSDateTime.Min = (u8)(Seconds / SECONDS_PER_MINUTE); |
Seconds = Seconds % SECONDS_PER_HOUR; // remaining seconds of current minute |
GPSDateTime.Sec = (u8)(Seconds); |
GPSDateTime.mSec = (u16)(UbxSol.itow % 1000L); |
GPSDateTime.Status = NEWDATA; |
} |
else |
{ |
GPSDateTime.Status = INVALID; |
} |
} |
|
|
|
/********************************************************/ |
/* Connect RXD & TXD to GPS */ |
/********************************************************/ |
void Connect_UART0_to_GPS(void) |
307,10 → 395,11 |
GPS_Data.SatFix = UbxSol.GPSfix; |
GPS_Data.Position_Accuracy = UbxSol.PAcc; |
GPS_Data.Speed_Accuracy = UbxSol.SAcc; |
Update_GPSDateTime(UbxSol.week, UbxSol.itow); |
UbxSol.Status = PROCESSED; // ready for new data |
// NAV POSLLH |
GPS_Data.UpdateTime = UbxPosLlh.ITOW - lasttime; |
lasttime = UbxPosLlh.ITOW; |
GPS_Data.UpdateTime = UbxPosLlh.itow - lasttime; |
lasttime = UbxPosLlh.itow; |
GPS_Data.Longitude = UbxPosLlh.LON; |
GPS_Data.Latitude = UbxPosLlh.LAT; |
GPS_Data.Altitude = UbxPosLlh.HMSL; |