899
|
1 /**
|
|
2 ******************************************************************************
|
|
3 * @file uartProtocol_GNSS.c
|
|
4 * @author heinrichs weikamp gmbh
|
|
5 * @version V0.0.1
|
|
6 * @date 30-Sep-2024
|
|
7 * @brief Interface functionality operation of GNSS devices
|
|
8 *
|
|
9 @verbatim
|
|
10
|
|
11
|
|
12 @endverbatim
|
|
13 ******************************************************************************
|
|
14 * @attention
|
|
15 *
|
|
16 * <h2><center>© COPYRIGHT(c) 2024 heinrichs weikamp</center></h2>
|
|
17 *
|
|
18 ******************************************************************************
|
|
19 */
|
|
20 /* Includes ------------------------------------------------------------------*/
|
|
21
|
|
22 #include <string.h>
|
|
23 #include "scheduler.h"
|
|
24 #include <uartProtocol_GNSS.h>
|
|
25 #include "uart.h"
|
|
26 #include "GNSS.h"
|
919
|
27 #include "configuration.h"
|
|
28 #include "externalInterface.h"
|
899
|
29
|
932
|
30
|
|
31 #if defined ENABLE_GNSS || defined ENABLE_GNSS_SUPPORT || defined ENABLE_GPIO_V2
|
|
32
|
|
33 static uartGnssStatus_t gnssState = UART_GNSS_INIT;
|
|
34 static gnssRequest_s activeRequest = {0,0};
|
899
|
35
|
|
36 static receiveStateGnss_t rxState = GNSSRX_READY;
|
919
|
37 static uint8_t GnssConnected = 0; /* Binary indicator if a sensor is connected or not */
|
|
38 static uint8_t writeIndex = 0;
|
|
39 static uint8_t dataToRead = 0;
|
936
|
40 static uint8_t ReqPowerDown = 0;
|
899
|
41
|
|
42 void ConvertByteToHexString(uint8_t byte, char* str)
|
|
43 {
|
|
44 uint8_t worker = 0;
|
|
45 uint8_t digit = 0;
|
|
46 uint8_t digitCnt = 1;
|
|
47
|
|
48 worker = byte;
|
|
49 while((worker!=0) && (digitCnt != 255))
|
|
50 {
|
|
51 digit = worker % 16;
|
|
52 if( digit < 10)
|
|
53 {
|
|
54 digit += '0';
|
|
55 }
|
|
56 else
|
|
57 {
|
|
58 digit += 'A' - 10;
|
|
59 }
|
|
60 str[digitCnt--]= digit;
|
|
61 worker = worker / 16;
|
|
62 }
|
|
63 }
|
|
64
|
936
|
65 void uartGnss_ReqPowerDown(uint8_t request)
|
|
66 {
|
|
67 if(GnssConnected)
|
|
68 {
|
939
|
69 ReqPowerDown = request;
|
936
|
70 }
|
|
71 }
|
919
|
72
|
939
|
73 uint8_t uartGnss_isPowerDownRequested()
|
|
74 {
|
|
75 return ReqPowerDown;
|
|
76 }
|
|
77
|
932
|
78 uartGnssStatus_t uartGnss_GetState()
|
|
79 {
|
|
80 return gnssState;
|
|
81 }
|
|
82 void uartGnss_SetState(uartGnssStatus_t newState)
|
|
83 {
|
|
84 gnssState = newState;
|
|
85 }
|
|
86
|
|
87 void UART_Gnss_SendCmd(uint8_t GnssCmd)
|
919
|
88 {
|
922
|
89 const uint8_t* pData;
|
919
|
90 uint8_t txLength = 0;
|
|
91
|
|
92 switch (GnssCmd)
|
|
93 {
|
|
94 case GNSSCMD_LOADCONF_0: pData = configUBX;
|
|
95 txLength = sizeof(configUBX) / sizeof(uint8_t);
|
|
96 break;
|
|
97 case GNSSCMD_LOADCONF_1: pData = setNMEA410;
|
|
98 txLength = sizeof(setNMEA410) / sizeof(uint8_t);
|
|
99 break;
|
|
100 case GNSSCMD_LOADCONF_2: pData = setGNSS;
|
|
101 txLength = sizeof(setGNSS) / sizeof(uint8_t);
|
|
102 break;
|
939
|
103 case GNSSCMD_SETMOBILE: pData = setPortableMode;
|
|
104 txLength = sizeof(setPortableMode) / sizeof(uint8_t);
|
|
105 break;
|
919
|
106 case GNSSCMD_GET_PVT_DATA: pData = getPVTData;
|
|
107 txLength = sizeof(getPVTData) / sizeof(uint8_t);
|
|
108 break;
|
|
109 case GNSSCMD_GET_NAV_DATA: pData = getNavigatorData;
|
|
110 txLength = sizeof(getNavigatorData) / sizeof(uint8_t);
|
|
111 break;
|
932
|
112 case GNSSCMD_GET_NAVSAT_DATA: pData = getNavSat;
|
|
113 txLength = sizeof(getNavSat) / sizeof(uint8_t);
|
|
114 break;
|
936
|
115 case GNSSCMD_MODE_PWS: pData = setPowerLow;
|
|
116 txLength = sizeof(setPowerLow) / sizeof(uint8_t);
|
|
117 break;
|
|
118 case GNSSCMD_MODE_NORMAL: pData = setPowerNormal;
|
|
119 txLength = sizeof(setPowerNormal) / sizeof(uint8_t);
|
|
120 break;
|
|
121 case GNSSCMD_SET_CONFIG: pData = setConfig;
|
|
122 txLength = sizeof(setConfig) / sizeof(uint8_t);
|
|
123 break;
|
919
|
124 default:
|
|
125 break;
|
|
126 }
|
|
127 if(txLength != 0)
|
|
128 {
|
932
|
129 activeRequest.class = pData[2];
|
|
130 activeRequest.id = pData[3];
|
919
|
131 UART_SendCmdUbx(pData, txLength);
|
|
132 }
|
|
133 }
|
|
134
|
899
|
135 void uartGnss_Control(void)
|
|
136 {
|
919
|
137 static uint32_t warmupTick = 0;
|
932
|
138 static uint8_t dataToggle = 0;
|
|
139 uint8_t activeSensor = 0;
|
|
140 sUartComCtrl* pUartCtrl = UART_GetGnssCtrl();
|
899
|
141
|
932
|
142 if(pUartCtrl == &Uart1Ctrl)
|
899
|
143 {
|
932
|
144 activeSensor = externalInterface_GetActiveUartSensor();
|
|
145 gnssState = externalInterface_GetSensorState(activeSensor + EXT_INTERFACE_MUX_OFFSET);
|
|
146 }
|
|
147
|
|
148 switch (gnssState)
|
|
149 {
|
|
150 case UART_GNSS_INIT: gnssState = UART_GNSS_WARMUP;
|
919
|
151 warmupTick = HAL_GetTick();
|
932
|
152 UART_clearRxBuffer(pUartCtrl);
|
899
|
153 break;
|
919
|
154 case UART_GNSS_WARMUP: if(time_elapsed_ms(warmupTick,HAL_GetTick()) > 1000)
|
|
155 {
|
932
|
156 gnssState = UART_GNSS_LOADCONF_0;
|
919
|
157 }
|
899
|
158 break;
|
932
|
159 case UART_GNSS_LOADCONF_0: UART_Gnss_SendCmd(GNSSCMD_LOADCONF_0);
|
|
160 rxState = GNSSRX_DETECT_ACK_0;
|
|
161 break;
|
|
162 case UART_GNSS_LOADCONF_1: UART_Gnss_SendCmd(GNSSCMD_LOADCONF_1);
|
|
163 rxState = GNSSRX_DETECT_ACK_0;
|
|
164 break;
|
|
165 case UART_GNSS_LOADCONF_2: UART_Gnss_SendCmd(GNSSCMD_LOADCONF_2);
|
919
|
166 rxState = GNSSRX_DETECT_ACK_0;
|
|
167 break;
|
939
|
168 case UART_GNSS_SETMODE_MOBILE: UART_Gnss_SendCmd(GNSSCMD_LOADCONF_2);
|
|
169 rxState = GNSSRX_DETECT_ACK_0;
|
|
170 break;
|
936
|
171 case UART_GNSS_PWRDOWN: UART_Gnss_SendCmd(GNSSCMD_MODE_PWS);
|
|
172 rxState = GNSSRX_DETECT_ACK_0;
|
|
173 break;
|
|
174
|
939
|
175 case UART_GNSS_PWRUP: UART_Gnss_SendCmd(GNSSCMD_MODE_NORMAL);
|
936
|
176 rxState = GNSSRX_DETECT_ACK_0;
|
|
177 gnssState = UART_GNSS_PWRUP;
|
|
178 break;
|
|
179 case UART_GNSS_SETCONF: UART_Gnss_SendCmd(GNSSCMD_SET_CONFIG);
|
|
180 rxState = GNSSRX_DETECT_ACK_0;
|
|
181 break;
|
|
182
|
|
183 case UART_GNSS_IDLE: if(ReqPowerDown)
|
932
|
184 {
|
936
|
185 UART_Gnss_SendCmd(GNSSCMD_MODE_PWS);
|
|
186 gnssState = UART_GNSS_PWRDOWN;
|
|
187 rxState = GNSSRX_DETECT_ACK_0;
|
932
|
188 }
|
|
189 else
|
|
190 {
|
936
|
191 if(dataToggle)
|
|
192 {
|
|
193 UART_Gnss_SendCmd(GNSSCMD_GET_PVT_DATA);
|
|
194 gnssState = UART_GNSS_GET_PVT;
|
|
195 rxState = GNSSRX_DETECT_HEADER_0;
|
|
196 dataToggle = 0;
|
|
197 }
|
|
198 else
|
|
199 {
|
|
200 UART_Gnss_SendCmd(GNSSCMD_GET_NAVSAT_DATA);
|
|
201 gnssState = UART_GNSS_GET_SAT;
|
|
202 rxState = GNSSRX_DETECT_HEADER_0;
|
|
203 dataToggle = 1;
|
|
204 }
|
932
|
205 }
|
899
|
206 break;
|
|
207 default:
|
|
208 break;
|
|
209 }
|
932
|
210 if(pUartCtrl == &Uart1Ctrl)
|
|
211 {
|
|
212 externalInterface_SetSensorState(activeSensor + EXT_INTERFACE_MUX_OFFSET,gnssState);
|
|
213 }
|
919
|
214
|
899
|
215 }
|
|
216
|
936
|
217
|
919
|
218 void uartGnss_ProcessData(uint8_t data)
|
899
|
219 {
|
932
|
220 static uint16_t rxLength = 0;
|
|
221 static uint8_t ck_A = 0;
|
|
222 static uint8_t ck_B = 0;
|
|
223 static uint8_t ck_A_Ref = 0;
|
|
224 static uint8_t ck_B_Ref = 0;
|
936
|
225 uint8_t activeSensor = 0;
|
|
226
|
|
227 sUartComCtrl* pUartCtrl = UART_GetGnssCtrl();
|
|
228
|
|
229 if(pUartCtrl == &Uart1Ctrl)
|
|
230 {
|
|
231 activeSensor = externalInterface_GetActiveUartSensor();
|
|
232 gnssState = externalInterface_GetSensorState(activeSensor + EXT_INTERFACE_MUX_OFFSET);
|
|
233 }
|
932
|
234
|
919
|
235 GNSS_Handle.uartWorkingBuffer[writeIndex++] = data;
|
932
|
236 if((rxState >= GNSSRX_DETECT_HEADER_2) && (rxState < GNSSRX_READ_CK_A))
|
|
237 {
|
|
238 ck_A += data;
|
|
239 ck_B += ck_A;
|
|
240 }
|
|
241
|
919
|
242 switch(rxState)
|
899
|
243 {
|
919
|
244 case GNSSRX_DETECT_ACK_0:
|
|
245 case GNSSRX_DETECT_HEADER_0: if(data == 0xB5)
|
|
246 {
|
|
247 writeIndex = 0;
|
932
|
248 memset(GNSS_Handle.uartWorkingBuffer,0xff, sizeof(GNSS_Handle.uartWorkingBuffer));
|
919
|
249 GNSS_Handle.uartWorkingBuffer[writeIndex++] = data;
|
|
250 rxState++;
|
932
|
251 ck_A = 0;
|
|
252 ck_B = 0;
|
919
|
253 }
|
|
254 break;
|
|
255 case GNSSRX_DETECT_ACK_1:
|
|
256 case GNSSRX_DETECT_HEADER_1: if(data == 0x62)
|
|
257 {
|
|
258 rxState++;
|
|
259 }
|
|
260 else
|
|
261 {
|
|
262 rxState = GNSSRX_DETECT_HEADER_0;
|
|
263 }
|
|
264 break;
|
|
265 case GNSSRX_DETECT_ACK_2: if(data == 0x05)
|
|
266 {
|
|
267 rxState++;
|
|
268 }
|
|
269 else
|
|
270 {
|
|
271 rxState = GNSSRX_DETECT_HEADER_0;
|
|
272 }
|
|
273 break;
|
932
|
274 case GNSSRX_DETECT_ACK_3: if((data == 0x01))
|
919
|
275 {
|
936
|
276 rxState = GNSSRX_READY;
|
939
|
277 switch(gnssState)
|
936
|
278 {
|
939
|
279 case UART_GNSS_PWRUP: gnssState = UART_GNSS_IDLE;
|
|
280 break;
|
|
281 case UART_GNSS_PWRDOWN: rxState = GNSSRX_DETECT_ACK_0;
|
|
282 UART_Gnss_SendCmd(GNSSCMD_SET_CONFIG);
|
|
283 gnssState = UART_GNSS_SETCONF;
|
|
284 break;
|
|
285 case UART_GNSS_SETCONF: gnssState = UART_GNSS_INACTIVE;
|
|
286 break;
|
|
287 case UART_GNSS_LOADCONF_0:
|
|
288 case UART_GNSS_LOADCONF_1: gnssState++;
|
|
289 break;
|
|
290 case UART_GNSS_LOADCONF_2: gnssState = UART_GNSS_SETMODE_MOBILE;
|
|
291 break;
|
955
|
292 case UART_GNSS_SETMODE_MOBILE: rxState = GNSSRX_DETECT_ACK_0;
|
|
293 UART_Gnss_SendCmd(GNSSCMD_MODE_NORMAL);
|
|
294 gnssState = UART_GNSS_PWRUP;
|
939
|
295 break;
|
|
296 default:
|
|
297 break;
|
932
|
298 }
|
919
|
299 GnssConnected = 1;
|
|
300 }
|
|
301 else
|
|
302 {
|
|
303 rxState = GNSSRX_DETECT_HEADER_0;
|
|
304 }
|
|
305 break;
|
932
|
306 case GNSSRX_DETECT_HEADER_2: if(data == activeRequest.class)
|
919
|
307 {
|
|
308 rxState++;
|
|
309 }
|
|
310 else
|
|
311 {
|
|
312 rxState = GNSSRX_DETECT_HEADER_0;
|
|
313 }
|
|
314 break;
|
932
|
315 case GNSSRX_DETECT_HEADER_3: if(data == activeRequest.id)
|
|
316 {
|
|
317 rxState = GNSSRX_DETECT_LENGTH_0;
|
|
318 }
|
|
319 else
|
919
|
320 {
|
932
|
321 rxState = GNSSRX_DETECT_HEADER_0;
|
919
|
322 }
|
932
|
323 break;
|
|
324 case GNSSRX_DETECT_LENGTH_0: rxLength = GNSS_Handle.uartWorkingBuffer[4];
|
|
325 rxState = GNSSRX_DETECT_LENGTH_1;
|
|
326 break;
|
|
327 case GNSSRX_DETECT_LENGTH_1: rxLength += (GNSS_Handle.uartWorkingBuffer[5] << 8);
|
|
328 rxState = GNSSRX_READ_DATA;
|
|
329 dataToRead = rxLength;
|
|
330 break;
|
|
331 case GNSSRX_READ_DATA: if(dataToRead > 0)
|
919
|
332 {
|
|
333 dataToRead--;
|
|
334 }
|
932
|
335 if(dataToRead == 0)
|
919
|
336 {
|
932
|
337 rxState = GNSSRX_READ_CK_A;
|
919
|
338 }
|
|
339 break;
|
932
|
340 case GNSSRX_READ_CK_A: ck_A_Ref = data;
|
|
341 rxState++;
|
|
342 break;
|
|
343 case GNSSRX_READ_CK_B: ck_B_Ref = data;
|
|
344 if((ck_A_Ref == ck_A) && (ck_B_Ref == ck_B))
|
|
345 {
|
|
346 switch(gnssState)
|
|
347 {
|
|
348 case UART_GNSS_GET_PVT:GNSS_ParsePVTData(&GNSS_Handle);
|
|
349 break;
|
|
350 case UART_GNSS_GET_SAT: GNSS_ParseNavSatData(&GNSS_Handle);
|
|
351 break;
|
|
352 default:
|
|
353 break;
|
|
354 }
|
|
355 }
|
|
356 rxState = GNSSRX_DETECT_HEADER_0;
|
|
357 gnssState = UART_GNSS_IDLE;
|
|
358 break;
|
|
359
|
919
|
360 default: rxState = GNSSRX_READY;
|
|
361 break;
|
899
|
362 }
|
936
|
363 if(pUartCtrl == &Uart1Ctrl)
|
|
364 {
|
|
365 externalInterface_SetSensorState(activeSensor + EXT_INTERFACE_MUX_OFFSET,gnssState);
|
|
366 }
|
899
|
367 }
|
|
368
|
919
|
369 uint8_t uartGnss_isSensorConnected()
|
|
370 {
|
|
371 return GnssConnected;
|
|
372 }
|
|
373
|
899
|
374 #endif
|
|
375
|