Mercurial > public > ostc4

comparison Small_CPU/Src/GNSS.c @ 981:c6c781a2e85b default

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Merge into default
author heinrichsweikamp
date Tue, 11 Feb 2025 18:12:00 +0100
parents 3420e3ba698d
children
comparison
equal deleted inserted replaced
871:f7318457df4d 981:c6c781a2e85b
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
29 #include <string.h>
30 #include "GNSS.h"
31 #include "data_exchange.h"
32 #include "rtc.h"
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) {
69 for (int var = 0; var < 4; var++) {
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) {
80
81 static float searchCnt = 1.0;
82
83 RTC_TimeTypeDef sTimeNow;
84
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.
140
141 if((GNSS->fLat == 0.0) && (GNSS->fLon == 0.0))
142 {
143 GNSS->fLat = searchCnt++;
144 }
145
146 if(GNSS->alive & GNSS_ALIVE_STATE_ALIVE) /* alive */
147 {
148 GNSS->alive &= ~GNSS_ALIVE_STATE_ALIVE;
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 {
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;
170 }
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 */
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
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
238
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 }