|
887
|
1 /*
|
|
|
2 * GNSS.c
|
|
|
3 *
|
|
|
4 * Created on: 03.10.2020
|
|
|
5 * Author: SimpleMethod
|
|
|
6 *
|
|
|
7 *Copyright 2020 SimpleMethod
|
|
|
8 *
|
|
|
9 *Permission is hereby granted, free of charge, to any person obtaining a copy of
|
|
|
10 *this software and associated documentation files (the "Software"), to deal in
|
|
|
11 *the Software without restriction, including without limitation the rights to
|
|
|
12 *use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
|
|
|
13 *of the Software, and to permit persons to whom the Software is furnished to do
|
|
|
14 *so, subject to the following conditions:
|
|
|
15 *
|
|
|
16 *The above copyright notice and this permission notice shall be included in all
|
|
|
17 *copies or substantial portions of the Software.
|
|
|
18 *
|
|
|
19 *THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
|
20 *IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
|
21 *FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
|
22 *AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
|
23 *LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
|
24 *OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
|
|
25 *THE SOFTWARE.
|
|
|
26 ******************************************************************************
|
|
|
27 */
|
|
|
28
|
|
931
|
29 #include <string.h>
|
|
887
|
30 #include "GNSS.h"
|
|
947
|
31 #include "data_exchange.h"
|
|
955
|
32 #include "rtc.h"
|
|
887
|
33
|
|
|
34 union u_Short uShort;
|
|
|
35 union i_Short iShort;
|
|
|
36 union u_Long uLong;
|
|
|
37 union i_Long iLong;
|
|
|
38
|
|
|
39 /*!
|
|
|
40 * Structure initialization.
|
|
|
41 * @param GNSS Pointer to main GNSS structure.
|
|
|
42 * @param huart Pointer to uart handle.
|
|
|
43 */
|
|
|
44 void GNSS_Init(GNSS_StateHandle *GNSS, UART_HandleTypeDef *huart) {
|
|
|
45 GNSS->huart = huart;
|
|
|
46 GNSS->year = 0;
|
|
|
47 GNSS->month = 0;
|
|
|
48 GNSS->day = 0;
|
|
|
49 GNSS->hour = 0;
|
|
|
50 GNSS->min = 0;
|
|
|
51 GNSS->sec = 0;
|
|
|
52 GNSS->fixType = 0;
|
|
|
53 GNSS->lon = 0;
|
|
|
54 GNSS->lat = 0;
|
|
|
55 GNSS->height = 0;
|
|
|
56 GNSS->hMSL = 0;
|
|
|
57 GNSS->hAcc = 0;
|
|
|
58 GNSS->vAcc = 0;
|
|
|
59 GNSS->gSpeed = 0;
|
|
|
60 GNSS->headMot = 0;
|
|
|
61 }
|
|
|
62
|
|
|
63 /*!
|
|
|
64 * Parse data to unique chip ID standard.
|
|
|
65 * Look at: 32.19.1.1 u-blox 8 Receiver description
|
|
|
66 * @param GNSS Pointer to main GNSS structure.
|
|
|
67 */
|
|
|
68 void GNSS_ParseUniqID(GNSS_StateHandle *GNSS) {
|
|
942
|
69 for (int var = 0; var < 5; var++) {
|
|
887
|
70 GNSS->uniqueID[var] = GNSS_Handle.uartWorkingBuffer[10 + var];
|
|
|
71 }
|
|
|
72 }
|
|
|
73
|
|
|
74 /*!
|
|
|
75 * Parse data to navigation position velocity time solution standard.
|
|
|
76 * Look at: 32.17.15.1 u-blox 8 Receiver description.
|
|
|
77 * @param GNSS Pointer to main GNSS structure.
|
|
|
78 */
|
|
|
79 void GNSS_ParsePVTData(GNSS_StateHandle *GNSS) {
|
|
919
|
80
|
|
|
81 static float searchCnt = 1.0;
|
|
|
82
|
|
955
|
83 RTC_TimeTypeDef sTimeNow;
|
|
|
84
|
|
887
|
85 uShort.bytes[0] = GNSS_Handle.uartWorkingBuffer[10];
|
|
|
86 GNSS->yearBytes[0]=GNSS_Handle.uartWorkingBuffer[10];
|
|
|
87 uShort.bytes[1] = GNSS_Handle.uartWorkingBuffer[11];
|
|
|
88 GNSS->yearBytes[1]=GNSS_Handle.uartWorkingBuffer[11];
|
|
|
89 GNSS->year = uShort.uShort;
|
|
|
90 GNSS->month = GNSS_Handle.uartWorkingBuffer[12];
|
|
|
91 GNSS->day = GNSS_Handle.uartWorkingBuffer[13];
|
|
|
92 GNSS->hour = GNSS_Handle.uartWorkingBuffer[14];
|
|
|
93 GNSS->min = GNSS_Handle.uartWorkingBuffer[15];
|
|
|
94 GNSS->sec = GNSS_Handle.uartWorkingBuffer[16];
|
|
|
95 GNSS->fixType = GNSS_Handle.uartWorkingBuffer[26];
|
|
|
96
|
|
|
97 for (int var = 0; var < 4; ++var) {
|
|
|
98 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 30];
|
|
|
99 GNSS->lonBytes[var]= GNSS_Handle.uartWorkingBuffer[var + 30];
|
|
|
100 }
|
|
|
101 GNSS->lon = iLong.iLong;
|
|
|
102 GNSS->fLon=(float)iLong.iLong/10000000.0;
|
|
|
103 for (int var = 0; var < 4; ++var) {
|
|
|
104 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 34];
|
|
|
105 GNSS->latBytes[var]=GNSS_Handle.uartWorkingBuffer[var + 34];
|
|
|
106 }
|
|
|
107 GNSS->lat = iLong.iLong;
|
|
|
108 GNSS->fLat=(float)iLong.iLong/10000000.0;
|
|
|
109 for (int var = 0; var < 4; ++var) {
|
|
|
110 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 38];
|
|
|
111 }
|
|
|
112 GNSS->height = iLong.iLong;
|
|
|
113
|
|
|
114 for (int var = 0; var < 4; ++var) {
|
|
|
115 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 42];
|
|
|
116 GNSS->hMSLBytes[var] = GNSS_Handle.uartWorkingBuffer[var + 42];
|
|
|
117 }
|
|
|
118 GNSS->hMSL = iLong.iLong;
|
|
|
119
|
|
|
120 for (int var = 0; var < 4; ++var) {
|
|
|
121 uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 46];
|
|
|
122 }
|
|
|
123 GNSS->hAcc = uLong.uLong;
|
|
|
124
|
|
|
125 for (int var = 0; var < 4; ++var) {
|
|
|
126 uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 50];
|
|
|
127 }
|
|
|
128 GNSS->vAcc = uLong.uLong;
|
|
|
129
|
|
|
130 for (int var = 0; var < 4; ++var) {
|
|
|
131 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 66];
|
|
|
132 GNSS->gSpeedBytes[var] = GNSS_Handle.uartWorkingBuffer[var + 66];
|
|
|
133 }
|
|
|
134 GNSS->gSpeed = iLong.iLong;
|
|
|
135
|
|
|
136 for (int var = 0; var < 4; ++var) {
|
|
|
137 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 70];
|
|
|
138 }
|
|
|
139 GNSS->headMot = iLong.iLong * 1e-5; // todo I'm not sure this good options.
|
|
919
|
140
|
|
|
141 if((GNSS->fLat == 0.0) && (GNSS->fLon == 0.0))
|
|
|
142 {
|
|
|
143 GNSS->fLat = searchCnt++;
|
|
|
144 }
|
|
940
|
145
|
|
947
|
146 if(GNSS->alive & GNSS_ALIVE_STATE_ALIVE) /* alive */
|
|
|
147 {
|
|
955
|
148 GNSS->alive &= ~GNSS_ALIVE_STATE_ALIVE;
|
|
947
|
149 }
|
|
|
150 else
|
|
|
151 {
|
|
|
152 GNSS->alive |= GNSS_ALIVE_STATE_ALIVE;
|
|
|
153 }
|
|
|
154 if((GNSS_Handle.uartWorkingBuffer[17] & 0x03) == 0x03) /* date/time valid */
|
|
|
155 {
|
|
|
156 GNSS->alive |= GNSS_ALIVE_STATE_TIME;
|
|
|
157 }
|
|
|
158 else
|
|
|
159 {
|
|
955
|
160 GNSS->alive &= ~GNSS_ALIVE_STATE_TIME;
|
|
|
161 }
|
|
|
162
|
|
|
163 if(GNSS->fixType >= 2)
|
|
|
164 {
|
|
|
165 RTC_GetTime(&sTimeNow);
|
|
|
166 GNSS->alive |= GNSS_ALIVE_BACKUP_POS;
|
|
|
167 GNSS->last_fLat = GNSS->fLat;
|
|
|
168 GNSS->last_fLon = GNSS->fLon;
|
|
|
169 GNSS->last_hour = sTimeNow.Hours;
|
|
947
|
170 }
|
|
887
|
171 }
|
|
|
172
|
|
|
173 /*!
|
|
|
174 * Parse data to UTC time solution standard.
|
|
|
175 * Look at: 32.17.30.1 u-blox 8 Receiver description.
|
|
|
176 * @param GNSS Pointer to main GNSS structure.
|
|
|
177 */
|
|
931
|
178 void GNSS_ParseNavSatData(GNSS_StateHandle *GNSS) {
|
|
|
179
|
|
|
180 uint8_t loop = 0;
|
|
|
181 uint8_t searchIndex = 0;
|
|
|
182 uint8_t statIndex = 0; /* only 4 state information will be forwarded */
|
|
|
183 uint8_t signalQuality = 0;
|
|
|
184 GNSS->numSat = GNSS_Handle.uartWorkingBuffer[11];
|
|
|
185
|
|
|
186 memset(GNSS->statSat, 0, sizeof(GNSS->statSat));
|
|
|
187
|
|
|
188 if(GNSS->numSat > 0)
|
|
|
189 {
|
|
|
190 searchIndex = 0;
|
|
|
191 while((searchIndex < GNSS->numSat) && (statIndex < 4)) /* get good signal quality */
|
|
|
192 {
|
|
|
193 signalQuality = (GNSS_Handle.uartWorkingBuffer[22 + searchIndex * 12] & 0x7);
|
|
|
194 if(signalQuality > 4)
|
|
|
195 {
|
|
|
196 GNSS->statSat[statIndex++] = signalQuality;
|
|
|
197 }
|
|
|
198 if(statIndex == 4) break;
|
|
|
199 searchIndex++;
|
|
|
200 }
|
|
|
201 searchIndex = 0;
|
|
|
202 while((searchIndex < GNSS->numSat) && (statIndex < 4)) /* get medium signal quality */
|
|
|
203 {
|
|
|
204 signalQuality = (GNSS_Handle.uartWorkingBuffer[22 + searchIndex * 12] & 0x7);
|
|
|
205 if((signalQuality > 2) && (signalQuality <= 4))
|
|
|
206 {
|
|
|
207 GNSS->statSat[statIndex++] = signalQuality;
|
|
|
208 }
|
|
|
209 if(statIndex == 4) break;
|
|
|
210 searchIndex++;
|
|
|
211 }
|
|
|
212 searchIndex = 0;
|
|
|
213 while((searchIndex < GNSS->numSat) && (statIndex < 4)) /* get poor signal quality */
|
|
|
214 {
|
|
|
215 signalQuality = (GNSS_Handle.uartWorkingBuffer[22 + searchIndex * 12] & 0x7);
|
|
|
216 if(signalQuality <= 2)
|
|
|
217 {
|
|
|
218 GNSS->statSat[statIndex++] = signalQuality;
|
|
|
219 }
|
|
|
220 if(statIndex == 4) break;
|
|
|
221 searchIndex++;
|
|
|
222 }
|
|
|
223 loop++;
|
|
|
224 }
|
|
|
225 }
|
|
|
226
|
|
887
|
227 void GNSS_ParseNavigatorData(GNSS_StateHandle *GNSS) {
|
|
|
228 uShort.bytes[0] = GNSS_Handle.uartWorkingBuffer[18];
|
|
|
229 uShort.bytes[1] = GNSS_Handle.uartWorkingBuffer[19];
|
|
|
230 GNSS->year = uShort.uShort;
|
|
|
231 GNSS->month = GNSS_Handle.uartWorkingBuffer[20];
|
|
|
232 GNSS->day = GNSS_Handle.uartWorkingBuffer[21];
|
|
|
233 GNSS->hour = GNSS_Handle.uartWorkingBuffer[22];
|
|
|
234 GNSS->min = GNSS_Handle.uartWorkingBuffer[23];
|
|
|
235 GNSS->sec = GNSS_Handle.uartWorkingBuffer[24];
|
|
|
236 }
|
|
|
237
|
|
931
|
238
|
|
887
|
239 /*!
|
|
|
240 * Parse data to geodetic position solution standard.
|
|
|
241 * Look at: 32.17.14.1 u-blox 8 Receiver description.
|
|
|
242 * @param GNSS Pointer to main GNSS structure.
|
|
|
243 */
|
|
|
244 void GNSS_ParsePOSLLHData(GNSS_StateHandle *GNSS) {
|
|
|
245 for (int var = 0; var < 4; ++var) {
|
|
|
246 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 10];
|
|
|
247 }
|
|
|
248 GNSS->lon = iLong.iLong;
|
|
|
249 GNSS->fLon=(float)iLong.iLong/10000000.0;
|
|
|
250
|
|
|
251 for (int var = 0; var < 4; ++var) {
|
|
|
252 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 14];
|
|
|
253 }
|
|
|
254 GNSS->lat = iLong.iLong;
|
|
|
255 GNSS->fLat=(float)iLong.iLong/10000000.0;
|
|
|
256
|
|
|
257 for (int var = 0; var < 4; ++var) {
|
|
|
258 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 18];
|
|
|
259 }
|
|
|
260 GNSS->height = iLong.iLong;
|
|
|
261
|
|
|
262 for (int var = 0; var < 4; ++var) {
|
|
|
263 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 22];
|
|
|
264 }
|
|
|
265 GNSS->hMSL = iLong.iLong;
|
|
|
266
|
|
|
267 for (int var = 0; var < 4; ++var) {
|
|
|
268 uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 26];
|
|
|
269 }
|
|
|
270 GNSS->hAcc = uLong.uLong;
|
|
|
271
|
|
|
272 for (int var = 0; var < 4; ++var) {
|
|
|
273 uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 30];
|
|
|
274 }
|
|
|
275 GNSS->vAcc = uLong.uLong;
|
|
|
276 }
|
