save some implementation

main/hw/gps.c

@@ -0,0 +1,136 @@
+#include "driver/uart.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "esp_log.h"
+#include "string.h"
+#include "gps.h"
+
+#define TAG "GPS"
+#define BUF_SIZE 1024
+#define GPS_LINE_MAX_LEN 128
+
+static QueueHandle_t uart_queue;
+
+// Initializes UART for GPS
+void gps_init()
+{
+    uart_config_t uart_config = {
+        .baud_rate = 9600,
+        .data_bits = UART_DATA_8_BITS,
+        .parity = UART_PARITY_DISABLE,
+        .stop_bits = UART_STOP_BITS_1,
+        .flow_ctrl = UART_HW_FLOWCTRL_CTS_RTS,
+        .rx_flow_ctrl_thresh = 122,
+    };
+    ESP_ERROR_CHECK(uart_param_config(GPS_UART_NUM, &uart_config));
+    ESP_ERROR_CHECK(uart_set_pin(GPS_UART_NUM, GPS_TXD, GPS_RXD, GPS_RTS, GPS_CTS));
+
+    const int uart_buffer_size = (1024 * 2);
+    ESP_ERROR_CHECK(uart_driver_install(GPS_UART_NUM, uart_buffer_size, uart_buffer_size, 10, &uart_queue, 0));
+}
+
+
+void gps_task(void *arg)
+{
+    uint8_t byte;
+    char line[GPS_LINE_MAX_LEN];
+    size_t line_pos = 0;
+
+    gps_init();
+
+    while (1) {
+        int len = uart_read_bytes(GPS_UART_NUM, &byte, 1, pdMS_TO_TICKS(1000));
+        if (len > 0) {
+            if (byte == '\n') {
+                line[line_pos] = '\0';
+
+                if (line[0] == '$') {
+                    ESP_LOGV(TAG, "Received NMEA: %s", line);
+
+                    if (strstr(line, "$GPGGA") == line) {
+                        parse_gpgga(line);
+                    } else if (strstr(line, "$GPRMC") == line) {
+                        parse_gprmc(line);
+                    }
+                }
+
+                line_pos = 0;
+            } else if (byte != '\r' && line_pos < (GPS_LINE_MAX_LEN - 1)) {
+                line[line_pos++] = byte;
+            } else if (line_pos >= GPS_LINE_MAX_LEN - 1) {
+                ESP_LOGW(TAG, "Line too long, discarded");
+                line_pos = 0;
+            }
+        }
+    }
+}
+
+void parse_gpgga(const char *nmea)
+{
+    char *fields[15];
+    char temp[GPS_LINE_MAX_LEN];
+    strncpy(temp, nmea, GPS_LINE_MAX_LEN);
+    temp[GPS_LINE_MAX_LEN - 1] = '\0';
+
+    int i = 0;
+    char *token = strtok(temp, ",");
+    while (token != NULL && i < 15) {
+        fields[i++] = token;
+        token = strtok(NULL, ",");
+    }
+
+    if (i < 10) return; // Not enough fields
+
+    // Time (hhmmss.sss)
+    const char *utc_time = fields[1];
+
+    // Latitude (ddmm.mmmm)
+    const char *lat = fields[2];
+    const char *lat_dir = fields[3];
+
+    // Longitude (dddmm.mmmm)
+    const char *lon = fields[4];
+    const char *lon_dir = fields[5];
+
+    // Fix quality (0 = invalid, 1 = GPS fix, 2 = DGPS fix)
+    const char *fix_quality = fields[6];
+
+    // Number of satellites
+    const char *num_satellites = fields[7];
+
+    // Altitude
+    const char *altitude = fields[9];
+
+    ESP_LOGI(TAG, "[GPGGA] Time: %s, Lat: %s %s, Lon: %s %s, Fix: %s, Sats: %s, Alt: %sm",
+             utc_time, lat, lat_dir, lon, lon_dir, fix_quality, num_satellites, altitude);
+}
+
+void parse_gprmc(const char *nmea)
+{
+    char *fields[13];
+    char temp[GPS_LINE_MAX_LEN];
+    strncpy(temp, nmea, GPS_LINE_MAX_LEN);
+    temp[GPS_LINE_MAX_LEN - 1] = '\0';
+
+    int i = 0;
+    char *token = strtok(temp, ",");
+    while (token != NULL && i < 13) {
+        fields[i++] = token;
+        token = strtok(NULL, ",");
+    }
+
+    if (i < 12) return;
+
+    const char *utc_time = fields[1];
+    const char *status = fields[2]; // A = active, V = void
+    const char *lat = fields[3];
+    const char *lat_dir = fields[4];
+    const char *lon = fields[5];
+    const char *lon_dir = fields[6];
+    const char *speed_knots = fields[7];
+    const char *date = fields[9];
+
+    ESP_LOGI(TAG, "[GPRMC] Date: %s, Time: %s, Lat: %s %s, Lon: %s %s, Speed: %s knots, Status: %s",
+             date, utc_time, lat, lat_dir, lon, lon_dir, speed_knots, status);
+}
+