Rev 434 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 434 | Rev 436 | ||
---|---|---|---|
1 | #include <inttypes.h> |
1 | #include <inttypes.h> |
2 | #include "ubx.h" |
2 | #include "ubx.h" |
3 | #include "timer0.h" |
3 | #include "timer0.h" |
4 | #include "uart0.h" |
4 | #include "uart0.h" |
- | 5 | #include "printf_P.h" |
|
5 | 6 | ||
6 | 7 | ||
7 | // ------------------------------------------------------------------------------------------------ |
8 | // ------------------------------------------------------------------------------------------------ |
8 | // defines |
9 | // defines |
9 | 10 | ||
10 | #define DAYS_FROM_JAN01YEAR0001_TO_JAN6_1980 722819 // the year 0 does not exist! |
11 | #define DAYS_FROM_JAN01YEAR0001_TO_JAN6_1980 722819 // the year 0 does not exist! |
11 | #define DAYS_PER_YEAR 365 |
12 | #define DAYS_PER_YEAR 365 |
12 | #define DAYS_PER_LEAPYEAR 366 |
13 | #define DAYS_PER_LEAPYEAR 366 |
13 | #define DAYS_PER_4YEARS 1461 //((3 * DAYS_PER_YEAR) + DAYS_PER_LEAPYEAR) // years dividable by 4 are leap years |
14 | #define DAYS_PER_4YEARS 1461 //((3 * DAYS_PER_YEAR) + DAYS_PER_LEAPYEAR) // years dividable by 4 are leap years |
14 | #define DAYS_PER_100YEARS 36524 //((25 * DAYS_PER_4YEARS) - 1) // years dividable by 100 are no leap years |
15 | #define DAYS_PER_100YEARS 36524 //((25 * DAYS_PER_4YEARS) - 1) // years dividable by 100 are no leap years |
15 | #define DAYS_PER_400YEARS 146097 //((4 * DAYS_PER_100YEARS) + 1L) // but years dividable by 400 are leap years |
16 | #define DAYS_PER_400YEARS 146097 //((4 * DAYS_PER_100YEARS) + 1L) // but years dividable by 400 are leap years |
16 | #define SECONDS_PER_MINUTE 60 |
17 | #define SECONDS_PER_MINUTE 60 |
17 | #define MINUTES_PER_HOUR 60 |
18 | #define MINUTES_PER_HOUR 60 |
18 | #define HOURS_PER_DAY 24 |
19 | #define HOURS_PER_DAY 24 |
19 | #define DAYS_PER_WEEK 7 |
20 | #define DAYS_PER_WEEK 7 |
20 | #define SECONDS_PER_HOUR 3600 //(SECONDS_PER_MINUTE * MINUTES_PER_HOUR) |
21 | #define SECONDS_PER_HOUR 3600 //(SECONDS_PER_MINUTE * MINUTES_PER_HOUR) |
21 | #define SECONDS_PER_DAY 86400 //(SECONDS_PER_HOUR * HOURS_PER_DAY) |
22 | #define SECONDS_PER_DAY 86400 //(SECONDS_PER_HOUR * HOURS_PER_DAY) |
22 | #define SECONDS_PER_WEEK 604800 //(SECONDS_PER_DAY * DAYS_PER_WEEK) |
23 | #define SECONDS_PER_WEEK 604800 //(SECONDS_PER_DAY * DAYS_PER_WEEK) |
23 | 24 | ||
24 | // days per month in normal and leap years |
25 | // days per month in normal and leap years |
25 | const uint32_t Leap[ 13 ] = { 0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335, 366 }; |
26 | const uint32_t Leap[ 13 ] = { 0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335, 366 }; |
26 | const uint32_t Normal[ 13 ] = { 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334, 365 }; |
27 | const uint32_t Normal[ 13 ] = { 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334, 365 }; |
27 | 28 | ||
28 | #define LEAP_SECONDS_FROM_1980 15 // the last one was on the Dec 31th 2008 |
29 | #define LEAP_SECONDS_FROM_1980 15 // the last one was on the Dec 31th 2008 |
29 | 30 | ||
30 | // message sync bytes |
31 | // message sync bytes |
31 | #define UBX_SYNC1_CHAR 0xB5 |
32 | #define UBX_SYNC1_CHAR 0xB5 |
32 | #define UBX_SYNC2_CHAR 0x62 |
33 | #define UBX_SYNC2_CHAR 0x62 |
33 | // protocoll identifier |
34 | // protocoll identifier |
34 | #define UBX_CLASS_NAV 0x01 |
35 | #define UBX_CLASS_NAV 0x01 |
35 | // message id |
36 | // message id |
36 | #define UBX_ID_POSLLH 0x02 |
37 | #define UBX_ID_POSLLH 0x02 |
37 | #define UBX_ID_SOL 0x06 |
38 | #define UBX_ID_SOL 0x06 |
38 | #define UBX_ID_VELNED 0x12 |
39 | #define UBX_ID_VELNED 0x12 |
39 | 40 | ||
40 | // ------------------------------------------------------------------------------------------------ |
41 | // ------------------------------------------------------------------------------------------------ |
41 | // typedefs |
42 | // typedefs |
42 | 43 | ||
43 | 44 | ||
44 | // ubx parser state |
45 | // ubx parser state |
45 | typedef enum |
46 | typedef enum |
46 | { |
47 | { |
47 | UBXSTATE_IDLE, |
48 | UBXSTATE_IDLE, |
48 | UBXSTATE_SYNC1, |
49 | UBXSTATE_SYNC1, |
49 | UBXSTATE_SYNC2, |
50 | UBXSTATE_SYNC2, |
50 | UBXSTATE_CLASS, |
51 | UBXSTATE_CLASS, |
51 | UBXSTATE_LEN1, |
52 | UBXSTATE_LEN1, |
52 | UBXSTATE_LEN2, |
53 | UBXSTATE_LEN2, |
53 | UBXSTATE_DATA, |
54 | UBXSTATE_DATA, |
54 | UBXSTATE_CKA, |
55 | UBXSTATE_CKA, |
55 | UBXSTATE_CKB |
56 | UBXSTATE_CKB |
56 | } ubxState_t; |
57 | } ubxState_t; |
57 | 58 | ||
58 | typedef struct |
59 | typedef struct |
59 | { |
60 | { |
60 | uint32_t itow; // ms GPS Millisecond Time of Week |
61 | uint32_t itow; // ms GPS Millisecond Time of Week |
61 | int32_t frac; // ns remainder of rounded ms above |
62 | int32_t frac; // ns remainder of rounded ms above |
62 | int16_t week; // GPS week |
63 | int16_t week; // GPS week |
63 | uint8_t GPSfix; // GPSfix Type, range 0..6 |
64 | uint8_t GPSfix; // GPSfix Type, range 0..6 |
64 | uint8_t Flags; // Navigation Status Flags |
65 | uint8_t Flags; // Navigation Status Flags |
65 | int32_t ECEF_X; // cm ECEF X coordinate |
66 | int32_t ECEF_X; // cm ECEF X coordinate |
66 | int32_t ECEF_Y; // cm ECEF Y coordinate |
67 | int32_t ECEF_Y; // cm ECEF Y coordinate |
67 | int32_t ECEF_Z; // cm ECEF Z coordinate |
68 | int32_t ECEF_Z; // cm ECEF Z coordinate |
68 | int32_t PAcc; // cm 3D Position Accuracy Estimate |
69 | int32_t PAcc; // cm 3D Position Accuracy Estimate |
69 | int32_t ECEFVX; // cm/s ECEF X velocity |
70 | int32_t ECEFVX; // cm/s ECEF X velocity |
70 | int32_t ECEFVY; // cm/s ECEF Y velocity |
71 | int32_t ECEFVY; // cm/s ECEF Y velocity |
71 | int32_t ECEFVZ; // cm/s ECEF Z velocity |
72 | int32_t ECEFVZ; // cm/s ECEF Z velocity |
72 | uint32_t SAcc; // cm/s Speed Accuracy Estimate |
73 | uint32_t SAcc; // cm/s Speed Accuracy Estimate |
73 | uint16_t PDOP; // 0.01 Position DOP |
74 | uint16_t PDOP; // 0.01 Position DOP |
74 | uint8_t res1; // reserved |
75 | uint8_t res1; // reserved |
75 | uint8_t numSV; // Number of SVs used in navigation solution |
76 | uint8_t numSV; // Number of SVs used in navigation solution |
76 | uint32_t res2; // reserved |
77 | uint32_t res2; // reserved |
77 | uint8_t Status; // invalid/newdata/processed |
78 | uint8_t Status; // invalid/newdata/processed |
78 | } __attribute__((packed)) ubx_nav_sol_t; |
79 | } __attribute__((packed)) ubx_nav_sol_t; |
79 | 80 | ||
80 | 81 | ||
81 | typedef struct |
82 | typedef struct |
82 | { |
83 | { |
83 | uint32_t itow; // ms GPS Millisecond Time of Week |
84 | uint32_t itow; // ms GPS Millisecond Time of Week |
84 | int32_t VEL_N; // cm/s NED north velocity |
85 | int32_t VEL_N; // cm/s NED north velocity |
85 | int32_t VEL_E; // cm/s NED east velocity |
86 | int32_t VEL_E; // cm/s NED east velocity |
86 | int32_t VEL_D; // cm/s NED down velocity |
87 | int32_t VEL_D; // cm/s NED down velocity |
87 | int32_t Speed; // cm/s Speed (3-D) |
88 | int32_t Speed; // cm/s Speed (3-D) |
88 | int32_t GSpeed; // cm/s Ground Speed (2-D) |
89 | int32_t GSpeed; // cm/s Ground Speed (2-D) |
89 | int32_t Heading; // 1e-05 deg Heading 2-D |
90 | int32_t Heading; // 1e-05 deg Heading 2-D |
90 | uint32_t SAcc; // cm/s Speed Accuracy Estimate |
91 | uint32_t SAcc; // cm/s Speed Accuracy Estimate |
91 | uint32_t CAcc; // deg Course / Heading Accuracy Estimate |
92 | uint32_t CAcc; // deg Course / Heading Accuracy Estimate |
92 | uint8_t Status; // invalid/newdata/processed |
93 | uint8_t Status; // invalid/newdata/processed |
93 | } __attribute__((packed)) ubx_nav_velned_t; |
94 | } __attribute__((packed)) ubx_nav_velned_t; |
94 | 95 | ||
95 | typedef struct |
96 | typedef struct |
96 | { |
97 | { |
97 | uint32_t itow; // ms GPS Millisecond Time of Week |
98 | uint32_t itow; // ms GPS Millisecond Time of Week |
98 | int32_t LON; // 1e-07 deg Longitude |
99 | int32_t LON; // 1e-07 deg Longitude |
99 | int32_t LAT; // 1e-07 deg Latitude |
100 | int32_t LAT; // 1e-07 deg Latitude |
100 | int32_t HEIGHT; // mm Height above Ellipsoid |
101 | int32_t HEIGHT; // mm Height above Ellipsoid |
101 | int32_t HMSL; // mm Height above mean sea level |
102 | int32_t HMSL; // mm Height above mean sea level |
102 | uint32_t Hacc; // mm Horizontal Accuracy Estimate |
103 | uint32_t Hacc; // mm Horizontal Accuracy Estimate |
103 | uint32_t Vacc; // mm Vertical Accuracy Estimate |
104 | uint32_t Vacc; // mm Vertical Accuracy Estimate |
104 | uint8_t Status; // invalid/newdata/processed |
105 | uint8_t Status; // invalid/newdata/processed |
105 | } __attribute__((packed)) ubx_nav_posllh_t; |
106 | } __attribute__((packed)) ubx_nav_posllh_t; |
106 | 107 | ||
107 | 108 | ||
108 | 109 | ||
109 | //------------------------------------------------------------------------------------ |
110 | //------------------------------------------------------------------------------------ |
110 | // global variables |
111 | // global variables |
111 | 112 | ||
112 | // local buffers for the incomming ubx messages |
113 | // local buffers for the incomming ubx messages |
113 | volatile ubx_nav_sol_t UbxSol = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID}; |
114 | volatile ubx_nav_sol_t UbxSol = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID}; |
114 | volatile ubx_nav_posllh_t UbxPosLlh = {0,0,0,0,0,0,0, INVALID}; |
115 | volatile ubx_nav_posllh_t UbxPosLlh = {0,0,0,0,0,0,0, INVALID}; |
115 | volatile ubx_nav_velned_t UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID}; |
116 | volatile ubx_nav_velned_t UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID}; |
116 | 117 | ||
117 | uint16_t CheckGPSOkay = 0; |
118 | uint16_t CheckGPSOkay = 0; |
118 | 119 | ||
119 | // shared buffer |
120 | // shared buffer |
120 | gps_data_t GPSData = {{0,0,0,INVALID},0,0,0,0,0,0,0, INVALID}; |
121 | gps_data_t GPSData = {{0,0,0,INVALID},0,0,0,0,0,0,0, INVALID}; |
121 | 122 | ||
122 | //------------------------------------------------------------------------------------ |
123 | //------------------------------------------------------------------------------------ |
123 | // functions |
124 | // functions |
124 | 125 | ||
125 | uint8_t IsLeapYear(uint16_t year) |
126 | uint8_t IsLeapYear(uint16_t year) |
126 | { |
127 | { |
127 | if((year%400 == 0) || ( (year%4 == 0) && (year%100 != 0) ) ) return 1; |
128 | if((year%400 == 0) || ( (year%4 == 0) && (year%100 != 0) ) ) return 1; |
128 | else return 0; |
129 | else return 0; |
129 | } |
130 | } |
130 | 131 | ||
131 | /********************************************************/ |
132 | /********************************************************/ |
132 | /* Calculates the UTC Time from the GPS week and tow */ |
133 | /* Calculates the UTC Time from the GPS week and tow */ |
133 | /********************************************************/ |
134 | /********************************************************/ |
134 | void SetGPSTime(DateTime_t * pTimeStruct) |
135 | void SetGPSTime(DateTime_t * pTimeStruct) |
135 | { |
136 | { |
136 | uint32_t Days, Seconds, Week; |
137 | uint32_t Days, Seconds, Week; |
137 | uint16_t YearPart; |
138 | uint16_t YearPart; |
138 | uint32_t * MonthDayTab = 0; |
139 | uint32_t * MonthDayTab = 0; |
139 | uint8_t i; |
140 | uint8_t i; |
140 | 141 | ||
141 | 142 | ||
142 | 143 | ||
143 | // if GPS data show valid time data |
144 | // if GPS data show valid time data |
144 | if((UbxSol.Status != INVALID) && (UbxSol.Flags & FLAG_WKNSET) && (UbxSol.Flags & FLAG_TOWSET) ) |
145 | if((UbxSol.Status != INVALID) && (UbxSol.Flags & FLAG_WKNSET) && (UbxSol.Flags & FLAG_TOWSET) ) |
145 | { |
146 | { |
146 | Seconds = UbxSol.itow / 1000L; |
147 | Seconds = UbxSol.itow / 1000L; |
147 | Week = (uint32_t)UbxSol.week; |
148 | Week = (uint32_t)UbxSol.week; |
148 | // correct leap seconds since 1980 |
149 | // correct leap seconds since 1980 |
149 | if(Seconds < LEAP_SECONDS_FROM_1980) |
150 | if(Seconds < LEAP_SECONDS_FROM_1980) |
150 | { |
151 | { |
151 | Week--; |
152 | Week--; |
152 | Seconds = SECONDS_PER_WEEK - LEAP_SECONDS_FROM_1980 + Seconds; |
153 | Seconds = SECONDS_PER_WEEK - LEAP_SECONDS_FROM_1980 + Seconds; |
153 | } |
154 | } |
154 | else Seconds -= LEAP_SECONDS_FROM_1980; |
155 | else Seconds -= LEAP_SECONDS_FROM_1980; |
155 | 156 | ||
156 | Days = DAYS_FROM_JAN01YEAR0001_TO_JAN6_1980; |
157 | Days = DAYS_FROM_JAN01YEAR0001_TO_JAN6_1980; |
157 | Days += (Week * DAYS_PER_WEEK); |
158 | Days += (Week * DAYS_PER_WEEK); |
158 | Days += Seconds / SECONDS_PER_DAY; // seperate days from GPS seconds of week |
159 | Days += Seconds / SECONDS_PER_DAY; // seperate days from GPS seconds of week |
159 | 160 | ||
160 | pTimeStruct->Year = 1; |
161 | pTimeStruct->Year = 1; |
161 | YearPart = (uint16_t)(Days / DAYS_PER_400YEARS); |
162 | YearPart = (uint16_t)(Days / DAYS_PER_400YEARS); |
162 | pTimeStruct->Year += YearPart * 400; |
163 | pTimeStruct->Year += YearPart * 400; |
163 | Days = Days % DAYS_PER_400YEARS; |
164 | Days = Days % DAYS_PER_400YEARS; |
164 | YearPart = (uint16_t)(Days / DAYS_PER_100YEARS); |
165 | YearPart = (uint16_t)(Days / DAYS_PER_100YEARS); |
165 | pTimeStruct->Year += YearPart * 100; |
166 | pTimeStruct->Year += YearPart * 100; |
166 | Days = Days % DAYS_PER_100YEARS; |
167 | Days = Days % DAYS_PER_100YEARS; |
167 | YearPart = (uint16_t)(Days / DAYS_PER_4YEARS); |
168 | YearPart = (uint16_t)(Days / DAYS_PER_4YEARS); |
168 | pTimeStruct->Year += YearPart * 4; |
169 | pTimeStruct->Year += YearPart * 4; |
169 | Days = Days % DAYS_PER_4YEARS; |
170 | Days = Days % DAYS_PER_4YEARS; |
170 | if(Days < (3* DAYS_PER_YEAR)) YearPart = (uint16_t)(Days / DAYS_PER_YEAR); |
171 | if(Days < (3* DAYS_PER_YEAR)) YearPart = (uint16_t)(Days / DAYS_PER_YEAR); |
171 | else YearPart = 3; |
172 | else YearPart = 3; |
172 | pTimeStruct->Year += YearPart; |
173 | pTimeStruct->Year += YearPart; |
173 | // calculate remaining days of year |
174 | // calculate remaining days of year |
174 | Days -= (uint32_t)(YearPart * DAYS_PER_YEAR); |
175 | Days -= (uint32_t)(YearPart * DAYS_PER_YEAR); |
175 | Days += 1; |
176 | Days += 1; |
176 | // check if current year is a leap year |
177 | // check if current year is a leap year |
177 | if(IsLeapYear(pTimeStruct->Year)) MonthDayTab = (uint32_t*)Leap; |
178 | if(IsLeapYear(pTimeStruct->Year)) MonthDayTab = (uint32_t*)Leap; |
178 | else MonthDayTab = (uint32_t*)Normal; |
179 | else MonthDayTab = (uint32_t*)Normal; |
179 | // seperate month and day from days of year |
180 | // seperate month and day from days of year |
180 | for ( i = 0; i < 12; i++ ) |
181 | for ( i = 0; i < 12; i++ ) |
181 | { |
182 | { |
182 | if ( (MonthDayTab[i]< Days) && (Days <= MonthDayTab[i+1]) ) |
183 | if ( (MonthDayTab[i]< Days) && (Days <= MonthDayTab[i+1]) ) |
183 | { |
184 | { |
184 | pTimeStruct->Month = i+1; |
185 | pTimeStruct->Month = i+1; |
185 | pTimeStruct->Day = Days - MonthDayTab[i]; |
186 | pTimeStruct->Day = Days - MonthDayTab[i]; |
186 | i = 12; |
187 | i = 12; |
187 | } |
188 | } |
188 | } |
189 | } |
189 | Seconds = Seconds % SECONDS_PER_DAY; // remaining seconds of current day |
190 | Seconds = Seconds % SECONDS_PER_DAY; // remaining seconds of current day |
190 | pTimeStruct->Hour = (uint8_t)(Seconds / SECONDS_PER_HOUR); |
191 | pTimeStruct->Hour = (uint8_t)(Seconds / SECONDS_PER_HOUR); |
191 | Seconds = Seconds % SECONDS_PER_HOUR; // remaining seconds of current hour |
192 | Seconds = Seconds % SECONDS_PER_HOUR; // remaining seconds of current hour |
192 | pTimeStruct->Min = (uint8_t)(Seconds / SECONDS_PER_MINUTE); |
193 | pTimeStruct->Min = (uint8_t)(Seconds / SECONDS_PER_MINUTE); |
193 | Seconds = Seconds % SECONDS_PER_MINUTE; // remaining seconds of current minute |
194 | Seconds = Seconds % SECONDS_PER_MINUTE; // remaining seconds of current minute |
194 | pTimeStruct->Sec = (uint8_t)(Seconds); |
195 | pTimeStruct->Sec = (uint8_t)(Seconds); |
195 | pTimeStruct->mSec = (uint16_t)(UbxSol.itow % 1000L); |
196 | pTimeStruct->mSec = (uint16_t)(UbxSol.itow % 1000L); |
196 | pTimeStruct->Valid = 1; |
197 | pTimeStruct->Valid = 1; |
197 | } |
198 | } |
198 | else |
199 | else |
199 | { |
200 | { |
200 | pTimeStruct->Valid = 0; |
201 | pTimeStruct->Valid = 0; |
201 | } |
202 | } |
202 | } |
203 | } |
203 | 204 | ||
204 | 205 | ||
205 | 206 | ||
206 | /********************************************************/ |
207 | /********************************************************/ |
207 | /* Initialize UBX Parser */ |
208 | /* Initialize UBX Parser */ |
208 | /********************************************************/ |
209 | /********************************************************/ |
209 | void UBX_Init(void) |
210 | void UBX_Init(void) |
210 | { |
211 | { |
211 | printf("\r\n UBX init..."); |
212 | printf("\r\n UBX init..."); |
212 | // mark msg buffers invalid |
213 | // mark msg buffers invalid |
213 | UbxSol.Status = INVALID; |
214 | UbxSol.Status = INVALID; |
214 | UbxPosLlh.Status = INVALID; |
215 | UbxPosLlh.Status = INVALID; |
215 | UbxVelNed.Status = INVALID; |
216 | UbxVelNed.Status = INVALID; |
216 | GPSData.Status = INVALID; |
217 | GPSData.Status = INVALID; |
217 | printf("ok"); |
218 | printf("ok"); |
218 | } |
219 | } |
219 | 220 | ||
220 | /********************************************************/ |
221 | /********************************************************/ |
221 | /* Upate GPS data stcructure */ |
222 | /* Upate GPS data stcructure */ |
222 | /********************************************************/ |
223 | /********************************************************/ |
223 | void Update_GPSData (void) |
224 | void Update_GPSData (void) |
224 | { |
225 | { |
225 | static uint16_t Ubx_Timeout = 0; |
226 | static uint16_t Ubx_Timeout = 0; |
226 | static uint8_t Msg_Count = 0; |
227 | static uint8_t Msg_Count = 0; |
227 | 228 | ||
228 | // the timeout is used to detect the delay between two message sets |
229 | // the timeout is used to detect the delay between two message sets |
229 | // and is used for synchronisation so that always a set is collected |
230 | // and is used for synchronisation so that always a set is collected |
230 | // that belongs together |
231 | // that belongs together |
231 | // _______NAVSOL|POSLLH|VELNED|___________________NAVSOL|POSLLH|VELNED|_____________ |
232 | // _______NAVSOL|POSLLH|VELNED|___________________NAVSOL|POSLLH|VELNED|_____________ |
232 | // | 8ms | 8ms | 184 ms | | | |
233 | // | 8ms | 8ms | 184 ms | | | |
233 | // msg_count: 0 1 2 0 1 2 |
234 | // msg_count: 0 1 2 0 1 2 |
234 | 235 | ||
235 | if(CheckDelay(Ubx_Timeout)) Msg_Count = 0; |
236 | if(CheckDelay(Ubx_Timeout)) Msg_Count = 0; |
236 | else Msg_Count++; |
237 | else Msg_Count++; |
237 | Ubx_Timeout = SetDelay(100); // reset ubx msg timeout |
238 | Ubx_Timeout = SetDelay(100); // reset ubx msg timeout |
238 | 239 | ||
239 | // if a new set of ubx messages was collected |
240 | // if a new set of ubx messages was collected |
240 | if((Msg_Count >= 2)) |
241 | if((Msg_Count >= 2)) |
241 | { // if set is complete |
242 | { // if set is complete |
242 | if((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA) && (UbxVelNed.Status == NEWDATA)) |
243 | if((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA) && (UbxVelNed.Status == NEWDATA)) |
243 | { |
244 | { |
244 | CheckGPSOkay++; |
245 | CheckGPSOkay++; |
245 | // update GPS data only if the status is INVALID or PROCESSED and the last ubx message was received within less than 100 ms |
246 | // update GPS data only if the status is INVALID or PROCESSED and the last ubx message was received within less than 100 ms |
246 | if(GPSData.Status != NEWDATA) // if last data were processed |
247 | if(GPSData.Status != NEWDATA) // if last data were processed |
247 | { // wait for new data at all neccesary ubx messages |
248 | { // wait for new data at all neccesary ubx messages |
248 | GPSData.Status = INVALID; |
249 | GPSData.Status = INVALID; |
249 | // NAV SOL |
250 | // NAV SOL |
250 | GPSData.Flags = UbxSol.Flags; |
251 | GPSData.Flags = UbxSol.Flags; |
251 | GPSData.NumOfSats = UbxSol.numSV; |
252 | GPSData.NumOfSats = UbxSol.numSV; |
252 | GPSData.SatFix = UbxSol.GPSfix; |
253 | GPSData.SatFix = UbxSol.GPSfix; |
253 | GPSData.Position_Accuracy = UbxSol.PAcc; |
254 | GPSData.Position_Accuracy = UbxSol.PAcc; |
254 | GPSData.Speed_Accuracy = UbxSol.SAcc; |
255 | GPSData.Speed_Accuracy = UbxSol.SAcc; |
255 | SetGPSTime(&SystemTime); // update system time |
256 | SetGPSTime(&SystemTime); // update system time |
256 | // NAV POSLLH |
257 | // NAV POSLLH |
257 | GPSData.Position.Status = INVALID; |
258 | GPSData.Position.Status = INVALID; |
258 | GPSData.Position.Longitude = UbxPosLlh.LON; |
259 | GPSData.Position.Longitude = UbxPosLlh.LON; |
259 | GPSData.Position.Latitude = UbxPosLlh.LAT; |
260 | GPSData.Position.Latitude = UbxPosLlh.LAT; |
260 | GPSData.Position.Altitude = UbxPosLlh.HMSL; |
261 | GPSData.Position.Altitude = UbxPosLlh.HMSL; |
261 | GPSData.Position.Status = NEWDATA; |
262 | GPSData.Position.Status = NEWDATA; |
262 | // NAV VELNED |
263 | // NAV VELNED |
263 | GPSData.Speed_East = UbxVelNed.VEL_E; |
264 | GPSData.Speed_East = UbxVelNed.VEL_E; |
264 | GPSData.Speed_North = UbxVelNed.VEL_N; |
265 | GPSData.Speed_North = UbxVelNed.VEL_N; |
265 | GPSData.Speed_Top = -UbxVelNed.VEL_D; |
266 | GPSData.Speed_Top = -UbxVelNed.VEL_D; |
266 | GPSData.Speed_Ground = UbxVelNed.GSpeed; |
267 | GPSData.Speed_Ground = UbxVelNed.GSpeed; |
267 | GPSData.Heading = UbxVelNed.Heading; |
268 | GPSData.Heading = UbxVelNed.Heading; |
268 | 269 | ||
269 | GPSData.Status = NEWDATA; // new data available |
270 | GPSData.Status = NEWDATA; // new data available |
270 | } // EOF if(GPSData.Status != NEWDATA) |
271 | } // EOF if(GPSData.Status != NEWDATA) |
271 | } // EOF all ubx messages received |
272 | } // EOF all ubx messages received |
272 | // set state to collect new data |
273 | // set state to collect new data |
273 | UbxSol.Status = PROCESSED; // ready for new data |
274 | UbxSol.Status = PROCESSED; // ready for new data |
274 | UbxPosLlh.Status = PROCESSED; // ready for new data |
275 | UbxPosLlh.Status = PROCESSED; // ready for new data |
275 | UbxVelNed.Status = PROCESSED; // ready for new data |
276 | UbxVelNed.Status = PROCESSED; // ready for new data |
276 | } |
277 | } |
277 | } |
278 | } |
278 | 279 | ||
279 | 280 | ||
280 | /********************************************************/ |
281 | /********************************************************/ |
281 | /* UBX Parser */ |
282 | /* UBX Parser */ |
282 | /********************************************************/ |
283 | /********************************************************/ |
283 | void UBX_Parser(uint8_t c) |
284 | void UBX_Parser(uint8_t c) |
284 | { |
285 | { |
285 | static ubxState_t ubxState = UBXSTATE_IDLE; |
286 | static ubxState_t ubxState = UBXSTATE_IDLE; |
286 | static uint16_t msglen; |
287 | static uint16_t msglen; |
287 | static uint8_t cka, ckb; |
288 | static uint8_t cka, ckb; |
288 | static uint8_t *ubxP, *ubxEp, *ubxSp; // pointers to data currently transfered |
289 | static uint8_t *ubxP, *ubxEp, *ubxSp; // pointers to data currently transfered |
289 | 290 | ||
290 | 291 | ||
291 | //state machine |
292 | //state machine |
292 | switch (ubxState) // ubx message parser |
293 | switch (ubxState) // ubx message parser |
293 | { |
294 | { |
294 | case UBXSTATE_IDLE: // check 1st sync byte |
295 | case UBXSTATE_IDLE: // check 1st sync byte |
295 | if (c == UBX_SYNC1_CHAR) ubxState = UBXSTATE_SYNC1; |
296 | if (c == UBX_SYNC1_CHAR) ubxState = UBXSTATE_SYNC1; |
296 | else ubxState = UBXSTATE_IDLE; // out of synchronization |
297 | else ubxState = UBXSTATE_IDLE; // out of synchronization |
297 | break; |
298 | break; |
298 | 299 | ||
299 | case UBXSTATE_SYNC1: // check 2nd sync byte |
300 | case UBXSTATE_SYNC1: // check 2nd sync byte |
300 | if (c == UBX_SYNC2_CHAR) ubxState = UBXSTATE_SYNC2; |
301 | if (c == UBX_SYNC2_CHAR) ubxState = UBXSTATE_SYNC2; |
301 | else ubxState = UBXSTATE_IDLE; // out of synchronization |
302 | else ubxState = UBXSTATE_IDLE; // out of synchronization |
302 | break; |
303 | break; |
303 | 304 | ||
304 | case UBXSTATE_SYNC2: // check msg class to be NAV |
305 | case UBXSTATE_SYNC2: // check msg class to be NAV |
305 | if (c == UBX_CLASS_NAV) ubxState = UBXSTATE_CLASS; |
306 | if (c == UBX_CLASS_NAV) ubxState = UBXSTATE_CLASS; |
306 | else ubxState = UBXSTATE_IDLE; // unsupported message class |
307 | else ubxState = UBXSTATE_IDLE; // unsupported message class |
307 | break; |
308 | break; |
308 | 309 | ||
309 | case UBXSTATE_CLASS: // check message identifier |
310 | case UBXSTATE_CLASS: // check message identifier |
310 | switch(c) |
311 | switch(c) |
311 | { |
312 | { |
312 | case UBX_ID_POSLLH: // geodetic position |
313 | case UBX_ID_POSLLH: // geodetic position |
313 | ubxP = (uint8_t *)&UbxPosLlh; // data start pointer |
314 | ubxP = (uint8_t *)&UbxPosLlh; // data start pointer |
314 | ubxEp = (uint8_t *)(&UbxPosLlh + 1); // data end pointer |
315 | ubxEp = (uint8_t *)(&UbxPosLlh + 1); // data end pointer |
315 | ubxSp = (uint8_t *)&UbxPosLlh.Status; // status pointer |
316 | ubxSp = (uint8_t *)&UbxPosLlh.Status; // status pointer |
316 | break; |
317 | break; |
317 | 318 | ||
318 | case UBX_ID_SOL: // navigation solution |
319 | case UBX_ID_SOL: // navigation solution |
319 | ubxP = (uint8_t *)&UbxSol; // data start pointer |
320 | ubxP = (uint8_t *)&UbxSol; // data start pointer |
320 | ubxEp = (uint8_t *)(&UbxSol + 1); // data end pointer |
321 | ubxEp = (uint8_t *)(&UbxSol + 1); // data end pointer |
321 | ubxSp = (uint8_t *)&UbxSol.Status; // status pointer |
322 | ubxSp = (uint8_t *)&UbxSol.Status; // status pointer |
322 | break; |
323 | break; |
323 | 324 | ||
324 | case UBX_ID_VELNED: // velocity vector in tangent plane |
325 | case UBX_ID_VELNED: // velocity vector in tangent plane |
325 | ubxP = (uint8_t *)&UbxVelNed; // data start pointer |
326 | ubxP = (uint8_t *)&UbxVelNed; // data start pointer |
326 | ubxEp = (uint8_t *)(&UbxVelNed + 1); // data end pointer |
327 | ubxEp = (uint8_t *)(&UbxVelNed + 1); // data end pointer |
327 | ubxSp = (uint8_t *)&UbxVelNed.Status; // status pointer |
328 | ubxSp = (uint8_t *)&UbxVelNed.Status; // status pointer |
328 | break; |
329 | break; |
329 | 330 | ||
330 | default: // unsupported identifier |
331 | default: // unsupported identifier |
331 | ubxState = UBXSTATE_IDLE; |
332 | ubxState = UBXSTATE_IDLE; |
332 | break; |
333 | break; |
333 | } |
334 | } |
334 | if (ubxState != UBXSTATE_IDLE) |
335 | if (ubxState != UBXSTATE_IDLE) |
335 | { |
336 | { |
336 | ubxState = UBXSTATE_LEN1; |
337 | ubxState = UBXSTATE_LEN1; |
337 | cka = UBX_CLASS_NAV + c; |
338 | cka = UBX_CLASS_NAV + c; |
338 | ckb = UBX_CLASS_NAV + cka; |
339 | ckb = UBX_CLASS_NAV + cka; |
339 | } |
340 | } |
340 | break; |
341 | break; |
341 | 342 | ||
342 | case UBXSTATE_LEN1: // 1st message length byte |
343 | case UBXSTATE_LEN1: // 1st message length byte |
343 | msglen = (uint16_t)c; // lowbyte first |
344 | msglen = (uint16_t)c; // lowbyte first |
344 | cka += c; |
345 | cka += c; |
345 | ckb += cka; |
346 | ckb += cka; |
346 | ubxState = UBXSTATE_LEN2; |
347 | ubxState = UBXSTATE_LEN2; |
347 | break; |
348 | break; |
348 | 349 | ||
349 | case UBXSTATE_LEN2: // 2nd message length byte |
350 | case UBXSTATE_LEN2: // 2nd message length byte |
350 | msglen += ((uint16_t)c)<<8; // high byte last |
351 | msglen += ((uint16_t)c)<<8; // high byte last |
351 | cka += c; |
352 | cka += c; |
352 | ckb += cka; |
353 | ckb += cka; |
353 | // if the old data are not processed so far then break parsing now |
354 | // if the old data are not processed so far then break parsing now |
354 | // to avoid writing new data in ISR during reading by another function |
355 | // to avoid writing new data in ISR during reading by another function |
355 | if ( *ubxSp == NEWDATA ) |
356 | if ( *ubxSp == NEWDATA ) |
356 | { |
357 | { |
357 | ubxState = UBXSTATE_IDLE; |
358 | ubxState = UBXSTATE_IDLE; |
358 | Update_GPSData(); //update GPS info respectively |
359 | Update_GPSData(); //update GPS info respectively |
359 | } |
360 | } |
360 | else // data invalid or allready processd |
361 | else // data invalid or allready processd |
361 | { |
362 | { |
362 | *ubxSp = INVALID; // mark invalid during buffer filling |
363 | *ubxSp = INVALID; // mark invalid during buffer filling |
363 | ubxState = UBXSTATE_DATA; |
364 | ubxState = UBXSTATE_DATA; |
364 | } |
365 | } |
365 | break; |
366 | break; |
366 | 367 | ||
367 | case UBXSTATE_DATA: // collecting data |
368 | case UBXSTATE_DATA: // collecting data |
368 | if (ubxP < ubxEp) |
369 | if (ubxP < ubxEp) |
369 | { |
370 | { |
370 | *ubxP++ = c; // copy curent data byte if any space is left |
371 | *ubxP++ = c; // copy curent data byte if any space is left |
371 | cka += c; |
372 | cka += c; |
372 | ckb += cka; |
373 | ckb += cka; |
373 | if (--msglen == 0) ubxState = UBXSTATE_CKA; // switch to next state if all data was read |
374 | if (--msglen == 0) ubxState = UBXSTATE_CKA; // switch to next state if all data was read |
374 | } |
375 | } |
375 | else // rx buffer overrun |
376 | else // rx buffer overrun |
376 | { |
377 | { |
377 | ubxState = UBXSTATE_IDLE; |
378 | ubxState = UBXSTATE_IDLE; |
378 | } |
379 | } |
379 | break; |
380 | break; |
380 | 381 | ||
381 | case UBXSTATE_CKA: |
382 | case UBXSTATE_CKA: |
382 | if (c == cka) ubxState = UBXSTATE_CKB; |
383 | if (c == cka) ubxState = UBXSTATE_CKB; |
383 | else |
384 | else |
384 | { |
385 | { |
385 | *ubxSp = INVALID; |
386 | *ubxSp = INVALID; |
386 | ubxState = UBXSTATE_IDLE; |
387 | ubxState = UBXSTATE_IDLE; |
387 | } |
388 | } |
388 | break; |
389 | break; |
389 | 390 | ||
390 | case UBXSTATE_CKB: |
391 | case UBXSTATE_CKB: |
391 | if (c == ckb) |
392 | if (c == ckb) |
392 | { |
393 | { |
393 | *ubxSp = NEWDATA; // new data are valid |
394 | *ubxSp = NEWDATA; // new data are valid |
394 | Update_GPSData(); //update GPS info respectively |
395 | Update_GPSData(); //update GPS info respectively |
395 | } |
396 | } |
396 | else |
397 | else |
397 | { // if checksum not match then set data invalid |
398 | { // if checksum not match then set data invalid |
398 | *ubxSp = INVALID; |
399 | *ubxSp = INVALID; |
399 | } |
400 | } |
400 | ubxState = UBXSTATE_IDLE; // ready to parse new data |
401 | ubxState = UBXSTATE_IDLE; // ready to parse new data |
401 | break; |
402 | break; |
402 | 403 | ||
403 | default: // unknown ubx state |
404 | default: // unknown ubx state |
404 | ubxState = UBXSTATE_IDLE; |
405 | ubxState = UBXSTATE_IDLE; |
405 | break; |
406 | break; |
406 | 407 | ||
407 | } |
408 | } |
408 | DebugOut.Analog[9] = GPSData.Speed_North; |
- | |
409 | DebugOut.Analog[10] = GPSData.Speed_East; |
- | |
410 | DebugOut.Analog[11] = GPSData.Speed_Top; |
- | |
411 | DebugOut.Analog[12] = GPSData.NumOfSats; |
- | |
412 | DebugOut.Analog[13] = GPSData.Position.Longitude; |
- | |
413 | DebugOut.Analog[14] = GPSData.Position.Latitude; |
- | |
414 | DebugOut.Analog[15] = GPSData.Position.Altitude; |
- | |
415 | } |
409 | } |
416 | 410 |