119 lines
3.3 KiB
C
119 lines
3.3 KiB
C
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
#include "ql_gnss.h"
|
|
#include "ql_rtos.h"
|
|
|
|
#include "factory_gnss.h"
|
|
|
|
#define FT_GNSS_NMEA_GGA_GSV ((1 << 0) | (1 << 2))
|
|
|
|
typedef struct {
|
|
volatile int open_ret;
|
|
volatile u32 nmea_count;
|
|
volatile int sat_count;
|
|
volatile int fix_status;
|
|
volatile int started;
|
|
u32 start_ms;
|
|
} factory_gnss_context_t;
|
|
|
|
static factory_gnss_context_t g_factory_gnss;
|
|
|
|
static u32 factory_gnss_now_ms(void)
|
|
{
|
|
return ql_rtos_get_systicks() * 5U;
|
|
}
|
|
|
|
static int nmea_field_int(const char *line, int line_len, int wanted_field,
|
|
int *value)
|
|
{
|
|
int field = 0;
|
|
int i;
|
|
int parsed = 0;
|
|
int number = 0;
|
|
|
|
for (i = 0; i < line_len; ++i) {
|
|
char ch = line[i];
|
|
if (ch == ',') {
|
|
if (field == wanted_field) break;
|
|
++field;
|
|
continue;
|
|
}
|
|
if (field == wanted_field) {
|
|
if (ch < '0' || ch > '9') break;
|
|
number = number * 10 + (ch - '0');
|
|
parsed = 1;
|
|
}
|
|
}
|
|
if (!parsed) return 0;
|
|
*value = number;
|
|
return 1;
|
|
}
|
|
|
|
static void factory_gnss_callback(char *data, int len)
|
|
{
|
|
int i;
|
|
|
|
if (!data || len <= 0) return;
|
|
for (i = 0; i + 5 < len; ++i) {
|
|
int line_end;
|
|
int value;
|
|
|
|
if (data[i] != '$') continue;
|
|
++g_factory_gnss.nmea_count;
|
|
for (line_end = i; line_end < len; ++line_end) {
|
|
if (data[line_end] == '\r' || data[line_end] == '\n') break;
|
|
}
|
|
if (data[i + 3] == 'G' && data[i + 4] == 'S' && data[i + 5] == 'V' &&
|
|
nmea_field_int(data + i, line_end - i, 3, &value) &&
|
|
value > g_factory_gnss.sat_count) {
|
|
g_factory_gnss.sat_count = value;
|
|
}
|
|
if (data[i + 3] == 'G' && data[i + 4] == 'G' && data[i + 5] == 'A' &&
|
|
nmea_field_int(data + i, line_end - i, 6, &value) &&
|
|
value > g_factory_gnss.fix_status) {
|
|
g_factory_gnss.fix_status = value;
|
|
}
|
|
i = line_end;
|
|
}
|
|
}
|
|
|
|
void factory_gnss_start(void)
|
|
{
|
|
int cfgsys_ret;
|
|
int cfgmsg_ret;
|
|
int mode_ret;
|
|
|
|
memset(&g_factory_gnss, 0, sizeof(g_factory_gnss));
|
|
g_factory_gnss.open_ret = -1;
|
|
g_factory_gnss.start_ms = factory_gnss_now_ms();
|
|
cfgsys_ret = ql_gnss_cfgsys_set(1);
|
|
cfgmsg_ret = ql_gnss_cfgmsg_set(FT_GNSS_NMEA_GGA_GSV);
|
|
mode_ret = ql_set_gnss_nmea_mode(PACKET_MODE);
|
|
g_factory_gnss.open_ret = ql_gnss_open(factory_gnss_callback);
|
|
g_factory_gnss.started = (g_factory_gnss.open_ret == 0);
|
|
printf("[FT_GNSS] start open_ret=%d cfgsys_ret=%d cfgmsg_ret=%d mode_ret=%d\r\n",
|
|
g_factory_gnss.open_ret, cfgsys_ret, cfgmsg_ret, mode_ret);
|
|
}
|
|
|
|
void factory_gnss_snapshot(factory_gnss_result_t *result)
|
|
{
|
|
if (!result) return;
|
|
result->open_ret = g_factory_gnss.open_ret;
|
|
result->nmea_count = g_factory_gnss.nmea_count;
|
|
result->sat_count = g_factory_gnss.sat_count;
|
|
result->fix_status = g_factory_gnss.fix_status;
|
|
result->search_ms = factory_gnss_now_ms() - g_factory_gnss.start_ms;
|
|
}
|
|
|
|
void factory_gnss_stop(void)
|
|
{
|
|
int ret;
|
|
if (!g_factory_gnss.started) return;
|
|
ret = ql_gnss_close();
|
|
g_factory_gnss.started = 0;
|
|
printf("[FT_GNSS] stop ret=%d nmea=%lu sat=%d fix=%d\r\n", ret,
|
|
(unsigned long)g_factory_gnss.nmea_count,
|
|
g_factory_gnss.sat_count, g_factory_gnss.fix_status);
|
|
}
|