Rev 684 | Rev 722 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 684 | Rev 702 | ||
---|---|---|---|
Line 1... | Line 1... | ||
1 | #include <inttypes.h> |
1 | #include <inttypes.h> |
2 | #include "ubx.h" |
2 | #include "ubx.h" |
- | 3 | #include "main.h" |
|
- | 4 | #include <avr/io.h> |
|
Line 3... | Line 5... | ||
3 | 5 | ||
4 | // ubx protocol parser state machine |
6 | // ubx protocol parser state machine |
5 | #define UBXSTATE_IDLE 0 |
7 | #define UBXSTATE_IDLE 0 |
6 | #define UBXSTATE_SYNC1 1 |
8 | #define UBXSTATE_SYNC1 1 |
Line 28... | Line 30... | ||
28 | uint8_t Flags; // Navigation Status Flags |
30 | uint8_t Flags; // Navigation Status Flags |
29 | uint8_t DiffS; // Differential Status |
31 | uint8_t DiffS; // Differential Status |
30 | uint8_t res; // reserved |
32 | uint8_t res; // reserved |
31 | uint32_t TTFF; // Time to first fix (millisecond time tag) |
33 | uint32_t TTFF; // Time to first fix (millisecond time tag) |
32 | uint32_t MSSS; // Milliseconds since Startup / Reset |
34 | uint32_t MSSS; // Milliseconds since Startup / Reset |
33 | uint8_t packetStatus; |
35 | uint8_t Status; |
34 | } GPS_STATUS_t; |
36 | } GPS_STATUS_t; |
Line 35... | Line 37... | ||
35 | 37 | ||
36 | typedef struct { |
38 | typedef struct { |
37 | uint32_t ITOW; // ms GPS Millisecond Time of Week |
39 | uint32_t ITOW; // ms GPS Millisecond Time of Week |
38 | int32_t LON; // 1e-07 deg Longitude |
40 | int32_t LON; // 1e-07 deg Longitude |
39 | int32_t LAT; // 1e-07 deg Latitude |
41 | int32_t LAT; // 1e-07 deg Latitude |
40 | int32_t HEIGHT; // mm Height above Ellipsoid |
42 | int32_t HEIGHT; // mm Height above Ellipsoid |
41 | int32_t HMSL; // mm Height above mean sea level |
43 | int32_t HMSL; // mm Height above mean sea level |
42 | uint32_t Hacc; // mm Horizontal Accuracy Estimate |
44 | uint32_t Hacc; // mm Horizontal Accuracy Estimate |
43 | uint32_t Vacc; // mm Vertical Accuracy Estimate |
45 | uint32_t Vacc; // mm Vertical Accuracy Estimate |
44 | uint8_t packetStatus; |
46 | uint8_t Status; |
Line 45... | Line 47... | ||
45 | } GPS_POSLLH_t; |
47 | } GPS_POSLLH_t; |
46 | 48 | ||
47 | typedef struct { |
49 | typedef struct { |
48 | uint32_t ITOW; // ms GPS Millisecond Time of Week |
50 | uint32_t ITOW; // ms GPS Millisecond Time of Week |
49 | int32_t EAST; // cm UTM Easting |
51 | int32_t EAST; // cm UTM Easting |
50 | int32_t NORTH; // cm UTM Nording |
52 | int32_t NORTH; // cm UTM Nording |
51 | int32_t ALT; // cm altitude |
53 | int32_t ALT; // cm altitude |
52 | uint8_t ZONE; // UTM zone number |
54 | uint8_t ZONE; // UTM zone number |
53 | uint8_t HEM; // Hemisphere Indicator (0=North, 1=South) |
55 | uint8_t HEM; // Hemisphere Indicator (0=North, 1=South) |
Line 54... | Line 56... | ||
54 | uint8_t packetStatus; |
56 | uint8_t Status; |
55 | } GPS_POSUTM_t; |
57 | } GPS_POSUTM_t; |
56 | 58 | ||
Line 62... | Line 64... | ||
62 | uint32_t Speed; // cm/s Speed (3-D) |
64 | uint32_t Speed; // cm/s Speed (3-D) |
63 | uint32_t GSpeed; // cm/s Ground Speed (2-D) |
65 | uint32_t GSpeed; // cm/s Ground Speed (2-D) |
64 | int32_t Heading; // 1e-05 deg Heading 2-D |
66 | int32_t Heading; // 1e-05 deg Heading 2-D |
65 | uint32_t SAcc; // cm/s Speed Accuracy Estimate |
67 | uint32_t SAcc; // cm/s Speed Accuracy Estimate |
66 | uint32_t CAcc; // deg Course / Heading Accuracy Estimate |
68 | uint32_t CAcc; // deg Course / Heading Accuracy Estimate |
67 | uint8_t packetStatus; |
69 | uint8_t Status; |
68 | } GPS_VELNED_t; |
70 | } GPS_VELNED_t; |
Line 69... | Line 71... | ||
69 | 71 | ||
70 | 72 | ||
71 | GPS_STATUS_t GpsStatus; |
73 | GPS_STATUS_t GpsStatus; |
72 | GPS_POSLLH_t GpsPosLlh; |
74 | GPS_POSLLH_t GpsPosLlh; |
Line 73... | Line 75... | ||
73 | GPS_POSUTM_t GpsPosUtm; |
75 | GPS_POSUTM_t GpsPosUtm; |
Line 74... | Line 76... | ||
74 | GPS_VELNED_t GpsVelNed; |
76 | GPS_VELNED_t GpsVelNed; |
75 | 77 | ||
76 | GPS_INFO_t actualPos; |
78 | GPS_INFO_t GPSInfo; |
77 | 79 | ||
78 | void UpdateGPSInfo (void) |
80 | void UpdateGPSInfo (void) |
79 | { |
81 | { |
80 | if (GpsStatus.packetStatus == VALID) // valid packet |
82 | if (GpsStatus.Status == VALID) // valid packet |
81 | { |
83 | { |
82 | actualPos.satfix = GpsStatus.GPSfix; |
84 | GPSInfo.satfix = GpsStatus.GPSfix; |
83 | GpsStatus.packetStatus = PROCESSED; // never update old data |
85 | GpsStatus.Status = PROCESSED; // never update old data |
84 | } |
86 | } |
85 | if (GpsPosLlh.packetStatus == VALID) // valid packet |
87 | if (GpsPosLlh.Status == VALID) // valid packet |
86 | { |
88 | { |
87 | actualPos.longitude = GpsPosLlh.LON; |
89 | GPSInfo.longitude = GpsPosLlh.LON; |
88 | actualPos.latitude = GpsPosLlh.LAT; |
90 | GPSInfo.latitude = GpsPosLlh.LAT; |
89 | actualPos.altitude = GpsPosLlh.HEIGHT; |
91 | GPSInfo.altitude = GpsPosLlh.HEIGHT; |
90 | GpsPosLlh.packetStatus = PROCESSED; // never update old data |
92 | GpsPosLlh.Status = PROCESSED; // never update old data |
91 | } |
93 | } |
92 | if (GpsPosUtm.packetStatus == VALID) // valid packet |
94 | if (GpsPosUtm.Status == VALID) // valid packet |
93 | { |
95 | { |
94 | actualPos.utmeast = GpsPosUtm.EAST; |
96 | GPSInfo.utmeast = GpsPosUtm.EAST; |
95 | actualPos.utmnorth = GpsPosUtm.NORTH; |
97 | GPSInfo.utmnorth = GpsPosUtm.NORTH; |
96 | actualPos.utmalt = GpsPosUtm.ALT; |
98 | GPSInfo.utmalt = GpsPosUtm.ALT; |
97 | GpsPosUtm.packetStatus = PROCESSED; // never update old data |
99 | GpsPosUtm.Status = PROCESSED; // never update old data |
98 | } |
100 | } |
99 | if (GpsVelNed.packetStatus == VALID) // valid packet |
101 | if (GpsVelNed.Status == VALID) // valid packet |
100 | { |
102 | { |
- | 103 | GPSInfo.veleast = GpsVelNed.VEL_E; |
|
- | 104 | GPSInfo.velnorth = GpsVelNed.VEL_N; |
|
- | 105 | GPSInfo.veltop = -GpsVelNed.VEL_D; |
|
- | 106 | GpsPosUtm.Status = PROCESSED; // never update old data |
|
101 | actualPos.veleast = GpsVelNed.VEL_E; |
107 | } |
102 | actualPos.velnorth = GpsVelNed.VEL_N; |
108 | if (GpsStatus.Status | GpsVelNed.Status | GpsPosLlh.Status | GpsPosUtm.Status) |
Line 103... | Line 109... | ||
103 | actualPos.veltop = -GpsVelNed.VEL_D; |
109 | { |
104 | GpsPosUtm.packetStatus = PROCESSED; // never update old data |
110 | GPSInfo.status = VALID; |
Line 134... | Line 140... | ||
134 | switch(c) |
140 | switch(c) |
135 | { |
141 | { |
136 | case UBX_ID_POSUTM: // utm position |
142 | case UBX_ID_POSUTM: // utm position |
137 | ubxP = (int8_t *)&GpsPosUtm; // data start pointer |
143 | ubxP = (int8_t *)&GpsPosUtm; // data start pointer |
138 | ubxEp = (int8_t *)(&GpsPosUtm + sizeof(GPS_POSUTM_t)); // data end pointer |
144 | ubxEp = (int8_t *)(&GpsPosUtm + sizeof(GPS_POSUTM_t)); // data end pointer |
139 | ubxSp = (int8_t *)&GpsPosUtm.packetStatus; // status pointer |
145 | ubxSp = (int8_t *)&GpsPosUtm.Status; // status pointer |
Line 140... | Line 146... | ||
140 | 146 | ||
Line 141... | Line 147... | ||
141 | break; |
147 | break; |
142 | 148 | ||
143 | case UBX_ID_POSLLH: // geodetic position |
149 | case UBX_ID_POSLLH: // geodetic position |
144 | ubxP = (int8_t *)&GpsStatus; // data start pointer |
150 | ubxP = (int8_t *)&GpsStatus; // data start pointer |
145 | ubxEp = (int8_t *)(&GpsStatus + sizeof(GPS_STATUS_t)); // data end pointer |
151 | ubxEp = (int8_t *)(&GpsStatus + sizeof(GPS_STATUS_t)); // data end pointer |
Line 146... | Line 152... | ||
146 | ubxSp = (int8_t *)&GpsStatus.packetStatus; // status pointer |
152 | ubxSp = (int8_t *)&GpsStatus.Status; // status pointer |
147 | break; |
153 | break; |
148 | 154 | ||
149 | case UBX_ID_STATUS: // receiver status |
155 | case UBX_ID_STATUS: // receiver status |
150 | ubxP = (int8_t *)&GpsStatus; // data start pointer |
156 | ubxP = (int8_t *)&GpsStatus; // data start pointer |
Line 151... | Line 157... | ||
151 | ubxEp = (int8_t *)(&GpsStatus + sizeof(GPS_STATUS_t)); // data end pointer |
157 | ubxEp = (int8_t *)(&GpsStatus + sizeof(GPS_STATUS_t)); // data end pointer |
152 | ubxSp = (int8_t *)&GpsStatus.packetStatus; // status pointer |
158 | ubxSp = (int8_t *)&GpsStatus.Status; // status pointer |
153 | break; |
159 | break; |
154 | 160 | ||
155 | case UBX_ID_VELNED: // velocity vector in tangent plane |
161 | case UBX_ID_VELNED: // velocity vector in tangent plane |
Line 156... | Line 162... | ||
156 | ubxP = (int8_t *)&GpsVelNed; // data start pointer |
162 | ubxP = (int8_t *)&GpsVelNed; // data start pointer |
157 | ubxEp = (int8_t *)(&GpsVelNed + sizeof(GPS_VELNED_t)); // data end pointer |
163 | ubxEp = (int8_t *)(&GpsVelNed + sizeof(GPS_VELNED_t)); // data end pointer |
158 | ubxSp = (int8_t *)&GpsVelNed.packetStatus; // status pointer |
164 | ubxSp = (int8_t *)&GpsVelNed.Status; // status pointer |
Line 206... | Line 212... | ||
206 | case UBXSTATE_CKB: |
212 | case UBXSTATE_CKB: |
207 | if (c == ckb) |
213 | if (c == ckb) |
208 | { |
214 | { |
209 | *ubxSp = VALID; // new data are availabe |
215 | *ubxSp = VALID; // new data are availabe |
210 | UpdateGPSInfo(); //update GPS info respectively |
216 | UpdateGPSInfo(); //update GPS info respectively |
- | 217 | ROT_FLASH; |
|
- | 218 | ||
211 | } |
219 | } |
212 | ubxstate = UBXSTATE_IDLE; // ready to parse new data |
220 | ubxstate = UBXSTATE_IDLE; // ready to parse new data |
213 | break; |
221 | break; |
Line 214... | Line 222... | ||
214 | 222 |