feat(factory): add GNSS test and LED status modes

2026-09-24 17:49:04 +08:00
parent c011f5c491
commit a4d6f79838
16 changed files with 361 additions and 76 deletions
@@ -0,0 +1,118 @@
+#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);
+}