forked from u-blox/ubxlib
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathu_gnss_info.c
407 lines (353 loc) · 17.1 KB
/
u_gnss_info.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
/*
* Copyright 2019-2024 u-blox
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
/* Only #includes of u_* and the C standard library are allowed here,
* no platform stuff and no OS stuff. Anything required from
* the platform/OS must be brought in through u_port* to maintain
* portability.
*/
/** @file
* @brief Implementation of the API to read general information from
* a GNSS chip; for position information please see the u_gnss_pos.h
* API instead.
*/
#ifdef U_CFG_OVERRIDE
# include "u_cfg_override.h" // For a customer's configuration override
#endif
#include "stddef.h" // NULL, size_t etc.
#include "stdint.h" // int32_t etc.
#include "stdbool.h"
#include "string.h" // memcpy()
#include "u_cfg_sw.h"
#include "u_error_common.h"
#include "u_port.h"
#include "u_port_os.h"
#include "u_port_heap.h"
#include "u_port_debug.h"
#include "u_time.h"
#include "u_at_client.h"
#include "u_ubx_protocol.h"
#include "u_gnss_module_type.h"
#include "u_gnss_type.h"
#include "u_gnss_private.h"
#include "u_gnss_msg.h" // uGnssMsgReceiveStatStreamLoss()
#include "u_gnss_info.h"
/* ----------------------------------------------------------------
* COMPILE-TIME MACROS
* -------------------------------------------------------------- */
/** The amount of space required to store the body of a
* UBX-MON-COMMS message (see uGnssInfoGetCommunicationStats())
* with max port numbers.
*/
#define U_GNSS_INFO_MESSAGE_BODY_LENGTH_UBX_MON_COMMS (8 + (40 * U_GNSS_PORT_MAX_NUM))
/* ----------------------------------------------------------------
* TYPES
* -------------------------------------------------------------- */
/* ----------------------------------------------------------------
* STATIC VARIABLES
* -------------------------------------------------------------- */
/* ----------------------------------------------------------------
* STATIC FUNCTIONS
* -------------------------------------------------------------- */
// Get the UTC time according to GNSS, with validity flags.
static int64_t getTimeUtc(uGnssPrivateInstance_t *pInstance,
uint8_t *pValidity)
{
int64_t errorCodeOrTime = (int64_t) U_ERROR_COMMON_INVALID_PARAMETER;
// Enough room for the body of the UBX-NAV-TIMEUTC message
char message[20];
int32_t months;
int32_t year;
if (pInstance != NULL) {
if (pValidity != NULL) {
*pValidity = 0;
}
// Poll with the message class and ID of the UBX-NAV-TIMEUTC command
errorCodeOrTime = uGnssPrivateSendReceiveUbxMessage(pInstance,
0x01, 0x21,
NULL, 0, message,
sizeof(message));
if (errorCodeOrTime >= (int64_t) sizeof(message)) {
if (pValidity != NULL) {
*pValidity = message[19];
}
errorCodeOrTime = 0;
// Year is 1999-2099, so need to adjust to get year since 1970
year = (uUbxProtocolUint16Decode(message + 12) - 1999) + 29;
// Month (1 to 12), so take away 1 to make it zero-based
months = message[14] - 1;
months += year * 12;
// Work out the number of seconds due to the year/month count
errorCodeOrTime += uTimeMonthsToSecondsUtc(months);
// Day (1 to 31)
errorCodeOrTime += ((int32_t) message[15] - 1) * 3600 * 24;
// Hour (0 to 23)
errorCodeOrTime += ((int32_t) message[16]) * 3600;
// Minute (0 to 59)
errorCodeOrTime += ((int32_t) message[17]) * 60;
// Second (0 to 60)
errorCodeOrTime += message[18];
uPortLog("U_GNSS_POS: UTC time is %d (validity 0x%02x).\n",
(int32_t) errorCodeOrTime, message[19]);
}
}
return errorCodeOrTime;
}
/* ----------------------------------------------------------------
* PUBLIC FUNCTIONS
* -------------------------------------------------------------- */
// Get the version string from the GNSS chip.
int32_t uGnssInfoGetFirmwareVersionStr(uDeviceHandle_t gnssHandle,
char *pStr, size_t size)
{
int32_t errorCodeOrLength = (int32_t) U_ERROR_COMMON_NOT_INITIALISED;
uGnssPrivateInstance_t *pInstance;
if (gUGnssPrivateMutex != NULL) {
U_PORT_MUTEX_LOCK(gUGnssPrivateMutex);
pInstance = pUGnssPrivateGetInstance(gnssHandle);
if (pInstance != NULL) {
// Poll with the message class and ID of the UBX-MON-VER
// message and pass the message body directly back
errorCodeOrLength = uGnssPrivateSendReceiveUbxMessage(pInstance,
0x0a, 0x04,
NULL, 0,
pStr, size);
// Add a terminator
if ((errorCodeOrLength > 0) && (size > 0)) {
if (errorCodeOrLength == size) {
errorCodeOrLength--;
}
*(pStr + errorCodeOrLength) = 0;
}
}
U_PORT_MUTEX_UNLOCK(gUGnssPrivateMutex);
}
return errorCodeOrLength;
}
int32_t uGnssInfoGetVersions(uDeviceHandle_t gnssHandle,
uGnssVersionType_t *pVer)
{
int32_t errorCodeOrLength = U_ERROR_COMMON_NOT_INITIALISED;
uGnssPrivateInstance_t *pInstance;
U_PORT_MUTEX_LOCK(gUGnssPrivateMutex);
if (gUGnssPrivateMutex != NULL) {
errorCodeOrLength = U_ERROR_COMMON_INVALID_PARAMETER;
pInstance = pUGnssPrivateGetInstance(gnssHandle);
if (pInstance != NULL) {
errorCodeOrLength = uGnssPrivateInfoGetVersions(pInstance, pVer);
}
U_PORT_MUTEX_UNLOCK(gUGnssPrivateMutex);
}
return errorCodeOrLength;
}
// Get the chip ID from the GNSS chip.
int32_t uGnssInfoGetIdStr(uDeviceHandle_t gnssHandle,
char *pStr, size_t size)
{
int32_t errorCodeOrLength = (int32_t) U_ERROR_COMMON_NOT_INITIALISED;
uGnssPrivateInstance_t *pInstance;
int32_t version;
// Enough room for the body of the UBX-SEC-UNIQID message
char message[10];
if (gUGnssPrivateMutex != NULL) {
U_PORT_MUTEX_LOCK(gUGnssPrivateMutex);
pInstance = pUGnssPrivateGetInstance(gnssHandle);
if (pInstance != NULL) {
// Poll with the message class and ID of the UBX-SEC-UNIQID command
errorCodeOrLength = uGnssPrivateSendReceiveUbxMessage(pInstance,
0x27, 0x03,
NULL, 0, message,
sizeof(message));
// For M9 and earlier (version 1) the body of the UBX-SEC-UNIQID
// message is only 9 bytes long, for M10 (version 2) it is 10 bytes
// long
if (errorCodeOrLength >= (int32_t) sizeof(message) - 1) {
// The first byte of the first uint32_t should indicate the version
version = uUbxProtocolUint32Decode(message) & 0xFF;
if ((version == 1) || (version == 2)) {
// The remaining bytes are the chip ID
errorCodeOrLength -= 4;
// Copy them in and add a terminator
if (size > 0) {
size--;
if (errorCodeOrLength > (int32_t) size) {
errorCodeOrLength = (int32_t) size;
}
if (pStr != NULL) {
memcpy(pStr, message + 4, errorCodeOrLength);
*(pStr + errorCodeOrLength) = 0;
}
} else {
errorCodeOrLength = 0;
}
} else {
errorCodeOrLength = (int32_t) U_ERROR_COMMON_PLATFORM;
}
}
}
U_PORT_MUTEX_UNLOCK(gUGnssPrivateMutex);
}
return errorCodeOrLength;
}
// Get the UTC time according to GNSS.
int64_t uGnssInfoGetTimeUtc(uDeviceHandle_t gnssHandle)
{
int64_t errorCodeOrTime = (int64_t) U_ERROR_COMMON_NOT_INITIALISED;
uGnssPrivateInstance_t *pInstance;
uint8_t validityFlags = 0;
if (gUGnssPrivateMutex != NULL) {
U_PORT_MUTEX_LOCK(gUGnssPrivateMutex);
errorCodeOrTime = (int64_t) U_ERROR_COMMON_INVALID_PARAMETER;
pInstance = pUGnssPrivateGetInstance(gnssHandle);
if (pInstance != NULL) {
errorCodeOrTime = getTimeUtc(pInstance, &validityFlags);
if ((validityFlags & 0x04) != 0x04 ) {
errorCodeOrTime = (int64_t) U_ERROR_COMMON_NOT_FOUND;
}
}
U_PORT_MUTEX_UNLOCK(gUGnssPrivateMutex);
}
return errorCodeOrTime;
}
// Get the UTC time according to GNSS without checking validity.
int64_t uGnssInfoGetTimeUtcRaw(uDeviceHandle_t gnssHandle)
{
int64_t errorCodeOrTime = (int64_t) U_ERROR_COMMON_NOT_INITIALISED;
uGnssPrivateInstance_t *pInstance;
if (gUGnssPrivateMutex != NULL) {
U_PORT_MUTEX_LOCK(gUGnssPrivateMutex);
errorCodeOrTime = (int64_t) U_ERROR_COMMON_INVALID_PARAMETER;
pInstance = pUGnssPrivateGetInstance(gnssHandle);
if (pInstance != NULL) {
errorCodeOrTime = getTimeUtc(pInstance, NULL);
}
U_PORT_MUTEX_UNLOCK(gUGnssPrivateMutex);
}
return errorCodeOrTime;
}
// Get the communication stats as seen by the GNSS chip.
int32_t uGnssInfoGetCommunicationStats(uDeviceHandle_t gnssHandle,
int32_t port,
uGnssCommunicationStats_t *pStats)
{
int32_t errorCode = (int32_t) U_ERROR_COMMON_NOT_INITIALISED;
uGnssPrivateInstance_t *pInstance;
char *pMessage;
int32_t messageLength;
int32_t numPorts = -1;
int32_t protocolId;
if (gUGnssPrivateMutex != NULL) {
U_PORT_MUTEX_LOCK(gUGnssPrivateMutex);
errorCode = (int32_t) U_ERROR_COMMON_INVALID_PARAMETER;
pInstance = pUGnssPrivateGetInstance(gnssHandle);
if (pInstance != NULL) {
errorCode = (int32_t) U_ERROR_COMMON_NOT_SUPPORTED;
// TODO: fix this properly with versioned UBX messaging later
if (pInstance->pModule->moduleType >= U_GNSS_MODULE_TYPE_M9) {
errorCode = (int32_t) U_ERROR_COMMON_NO_MEMORY;
// Message big enough to store UBX-MON-COMMS with max port numbers
pMessage = (char *) pUPortMalloc(U_GNSS_INFO_MESSAGE_BODY_LENGTH_UBX_MON_COMMS);
if (pMessage != NULL) {
if (port < 0) {
port = (int32_t) pInstance->portNumber;
}
// Note: the if() condition below is present to allow future
// values, or new and interesting values, to be passed transparently
// to this function.
if (port < U_GNSS_PORT_MAX_NUM) {
// The encoding of the port number in this message is _different_
// to that in UBX-CFG-PORT - here it is, adopting the form used
// in the system integration manuals, which is AFTER endian
// conversion:
//
// 0 ==> 0x0000 I2C
// 1 ==> 0x0100 UART1
// 2 ==> 0x0201 UART2
// 3 ==> 0x0300 USB
// 4 ==> 0x0400 SPI
//
// This is because there are additional UARTs internal to the
// GNSS device which need to be accounted for. The ones listed
// above are those that may be connected to a host MCU, but note
// that others (e.g. 0x0101) may appear in the output of
// UBX-MON-COMMS, which we will ignore.
port = ((uint32_t) port) << 8;
if (port == (((uint32_t) U_GNSS_PORT_UART2) << 8)) {
port++;
}
}
// Poll with the message class and ID of the UBX-MON-COMMS command
errorCode = uGnssPrivateSendReceiveUbxMessage(pInstance,
0x0a, 0x36,
NULL, 0, pMessage,
U_GNSS_INFO_MESSAGE_BODY_LENGTH_UBX_MON_COMMS);
if (errorCode >= 0) {
messageLength = errorCode;
if ((messageLength >= 2) && (*pMessage == 0)) {
// Have a message in a version we understand;
// get the number of ports reported in it
numPorts = *(pMessage + 1);
}
errorCode = (int32_t) U_ERROR_COMMON_DEVICE_ERROR;
if ((numPorts > 0) && (messageLength >= 8 + (numPorts * 40))) {
// The message has some ports in it and is of the correct
// length for that number of ports; run through the
// message in blocks of 40 bytes, the length of the
// report for one port being 40 bytes, after the initial
// 8 bytes, to find the report for our port number
for (int32_t offset = 0; (8 + offset < messageLength) &&
(errorCode != (int32_t) U_ERROR_COMMON_SUCCESS); offset += 40) {
// No endian conversion here as port is already endian converted
if (*(uint16_t *) (pMessage + 8 + offset) == port) {
errorCode = (int32_t) U_ERROR_COMMON_SUCCESS;
if (pStats != NULL) {
pStats->txPendingBytes = uUbxProtocolUint16Decode(pMessage + 10 + offset);
pStats->txBytes = uUbxProtocolUint32Decode(pMessage + 12 + offset);
pStats->txPercentageUsage = *(pMessage + 16 + offset);
pStats->txPeakPercentageUsage = *(pMessage + 17 + offset);
pStats->rxPendingBytes = uUbxProtocolUint16Decode(pMessage + 18 + offset);
pStats->rxBytes = uUbxProtocolUint32Decode(pMessage + 20 + offset);
pStats->rxPercentageUsage = *(pMessage + 24 + offset);
pStats->rxPeakPercentageUsage = *(pMessage + 25 + offset);
pStats->rxOverrunErrors = uUbxProtocolUint16Decode(pMessage + 26 + offset);
// The number of messages parsed is in the array which follows
// based on the array of protocol IDs way back at the start
// of the message in byte 4
for (size_t x = 0; x < sizeof(pStats->rxNumMessages) / sizeof(pStats->rxNumMessages[0]); x++) {
pStats->rxNumMessages[x] = -1;
}
for (size_t x = 0; x < 4; x++) {
protocolId = *(pMessage + 4 + x);
if ((protocolId >= 0) &&
(protocolId < sizeof(pStats->rxNumMessages) / sizeof(pStats->rxNumMessages[0]))) {
pStats->rxNumMessages[protocolId] = uUbxProtocolUint16Decode(pMessage + 28 + offset + (x * 2));
}
}
pStats->rxSkippedBytes = uUbxProtocolUint32Decode(pMessage + 44 + offset);
}
}
}
}
}
// Free memory
uPortFree(pMessage);
}
}
}
U_PORT_MUTEX_UNLOCK(gUGnssPrivateMutex);
}
return errorCode;
}
// End of file