diff --git a/components/gps_sync/CMakeLists.txt b/components/gps_sync/CMakeLists.txt new file mode 100644 index 0000000..48764ed --- /dev/null +++ b/components/gps_sync/CMakeLists.txt @@ -0,0 +1,5 @@ +idf_component_register( + SRCS "gps_sync.c" + INCLUDE_DIRS "include" + REQUIRES driver +) diff --git a/components/gps_sync/gps_sync.c b/components/gps_sync/gps_sync.c new file mode 100644 index 0000000..f9c37d4 --- /dev/null +++ b/components/gps_sync/gps_sync.c @@ -0,0 +1,310 @@ +#include "gps_sync.h" +#include "driver/gpio.h" +#include "driver/uart.h" +#include "esp_timer.h" +#include "esp_log.h" +#include +#include +#include +#include + +#define GPS_UART_NUM UART_NUM_1 +#define GPS_RX_PIN GPIO_NUM_4 +#define GPS_TX_PIN GPIO_NUM_5 +#define PPS_GPIO GPIO_NUM_1 +#define GPS_BAUD_RATE 9600 +#define UART_BUF_SIZE 1024 + +static const char *TAG = "GPS_SYNC"; + +// GPS sync state +static int64_t monotonic_offset_us = 0; +static volatile int64_t last_pps_monotonic = 0; +static volatile time_t next_pps_gps_second = 0; +static bool gps_has_fix = false; +static bool use_gps_for_logs = false; +static SemaphoreHandle_t sync_mutex; + +// For decimal timestamp formatting - stores last timestamp parts +static uint32_t last_timestamp_sec = 0; +static uint16_t last_timestamp_ms = 0; + +// PPS interrupt - captures exact monotonic time at second boundary +static void IRAM_ATTR pps_isr_handler(void* arg) { + last_pps_monotonic = esp_timer_get_time(); +} + +// Parse GPS time from NMEA sentence +static bool parse_gprmc(const char* nmea, struct tm* tm_out, bool* valid) { + if (strncmp(nmea, "$GPRMC", 6) != 0 && strncmp(nmea, "$GNRMC", 6) != 0) { + return false; + } + + char *p = strchr(nmea, ','); + if (!p) return false; + + // Time field + p++; + int hour, min, sec; + if (sscanf(p, "%2d%2d%2d", &hour, &min, &sec) != 3) { + return false; + } + + // Status field (A=valid, V=invalid) + p = strchr(p, ','); + if (!p) return false; + p++; + *valid = (*p == 'A'); + + // Skip to date field (8 commas ahead from time) + for (int i = 0; i < 7; i++) { + p = strchr(p, ','); + if (!p) return false; + p++; + } + + // Date field: ddmmyy + int day, month, year; + if (sscanf(p, "%2d%2d%2d", &day, &month, &year) != 3) { + return false; + } + + year += (year < 80) ? 2000 : 1900; + + tm_out->tm_sec = sec; + tm_out->tm_min = min; + tm_out->tm_hour = hour; + tm_out->tm_mday = day; + tm_out->tm_mon = month - 1; + tm_out->tm_year = year - 1900; + tm_out->tm_isdst = 0; + + return true; +} + +// GPS processing task +static void gps_task(void* arg) { + char line[128]; + int pos = 0; + + while (1) { + uint8_t data; + int len = uart_read_bytes(GPS_UART_NUM, &data, 1, 100 / portTICK_PERIOD_MS); + + if (len > 0) { + if (data == '\n') { + line[pos] = '\0'; + + struct tm gps_tm; + bool valid; + if (parse_gprmc(line, &gps_tm, &valid)) { + if (valid) { + time_t gps_time = mktime(&gps_tm); + + xSemaphoreTake(sync_mutex, portMAX_DELAY); + next_pps_gps_second = gps_time + 1; + xSemaphoreGive(sync_mutex); + + vTaskDelay(pdMS_TO_TICKS(300)); + + xSemaphoreTake(sync_mutex, portMAX_DELAY); + if (last_pps_monotonic > 0) { + int64_t gps_us = (int64_t)next_pps_gps_second * 1000000LL; + int64_t new_offset = gps_us - last_pps_monotonic; + + if (monotonic_offset_us == 0) { + monotonic_offset_us = new_offset; + } else { + // Low-pass filter: 90% old + 10% new + monotonic_offset_us = (monotonic_offset_us * 9 + new_offset) / 10; + } + + gps_has_fix = true; + + ESP_LOGI(TAG, "GPS sync: %04d-%02d-%02d %02d:%02d:%02d, offset=%lld us", + gps_tm.tm_year + 1900, gps_tm.tm_mon + 1, gps_tm.tm_mday, + gps_tm.tm_hour, gps_tm.tm_min, gps_tm.tm_sec, + monotonic_offset_us); + } + xSemaphoreGive(sync_mutex); + } else { + gps_has_fix = false; + } + } + + pos = 0; + } else if (pos < sizeof(line) - 1) { + line[pos++] = data; + } + } + } +} + +void gps_sync_init(bool use_gps_log_timestamps) { + ESP_LOGI(TAG, "Initializing GPS sync"); + + use_gps_for_logs = use_gps_log_timestamps; + + if (use_gps_log_timestamps) { + ESP_LOGI(TAG, "ESP_LOG timestamps: GPS time in seconds.milliseconds format"); + // Override vprintf to add decimal point to timestamps + esp_log_set_vprintf(gps_log_vprintf); + } + + sync_mutex = xSemaphoreCreateMutex(); + + uart_config_t uart_config = { + .baud_rate = GPS_BAUD_RATE, + .data_bits = UART_DATA_8_BITS, + .parity = UART_PARITY_DISABLE, + .stop_bits = UART_STOP_BITS_1, + .flow_ctrl = UART_HW_FLOWCTRL_DISABLE, + .source_clk = UART_SCLK_DEFAULT, + }; + + ESP_ERROR_CHECK(uart_driver_install(GPS_UART_NUM, UART_BUF_SIZE, 0, 0, NULL, 0)); + ESP_ERROR_CHECK(uart_param_config(GPS_UART_NUM, &uart_config)); + ESP_ERROR_CHECK(uart_set_pin(GPS_UART_NUM, GPS_TX_PIN, GPS_RX_PIN, + UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE)); + + gpio_config_t io_conf = { + .intr_type = GPIO_INTR_POSEDGE, + .mode = GPIO_MODE_INPUT, + .pin_bit_mask = (1ULL << PPS_GPIO), + .pull_up_en = GPIO_PULLUP_ENABLE, + .pull_down_en = GPIO_PULLDOWN_DISABLE, + }; + ESP_ERROR_CHECK(gpio_config(&io_conf)); + + ESP_ERROR_CHECK(gpio_install_isr_service(0)); + ESP_ERROR_CHECK(gpio_isr_handler_add(PPS_GPIO, pps_isr_handler, NULL)); + + xTaskCreate(gps_task, "gps_task", 4096, NULL, 5, NULL); + + ESP_LOGI(TAG, "GPS sync initialized (RX=GPIO%d, PPS=GPIO%d)", GPS_RX_PIN, PPS_GPIO); +} + +gps_timestamp_t gps_get_timestamp(void) { + gps_timestamp_t ts; + + // Using clock_gettime (POSIX standard, portable) + // ESP32 supports CLOCK_MONOTONIC for monotonic time + clock_gettime(CLOCK_MONOTONIC, &ts.mono_ts); + + xSemaphoreTake(sync_mutex, portMAX_DELAY); + + // Convert timespec to microseconds + ts.monotonic_us = (int64_t)ts.mono_ts.tv_sec * 1000000LL + + ts.mono_ts.tv_nsec / 1000; + + // Convert to milliseconds + ts.monotonic_ms = ts.monotonic_us / 1000; + + // Calculate GPS time + ts.gps_us = ts.monotonic_us + monotonic_offset_us; + ts.gps_ms = ts.gps_us / 1000; + + ts.synced = gps_has_fix; + xSemaphoreGive(sync_mutex); + + return ts; +} + +// Alternative: Get just milliseconds using clock_gettime +// Useful for simple logging where you only need millisecond resolution +int64_t gps_get_monotonic_ms(void) { + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + // Convert: seconds to ms + nanoseconds to ms + return (int64_t)ts.tv_sec * 1000LL + ts.tv_nsec / 1000000; +} + +bool gps_is_synced(void) { + return gps_has_fix; +} + +// Custom log timestamp function - returns value formatted as seconds*1000000 + milliseconds*1000 +// This allows us to extract both seconds and milliseconds when needed +// When printed directly, shows full milliseconds (we format it with decimal in custom logger) +uint32_t esp_log_timestamp(void) { + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + int64_t monotonic_us = (int64_t)ts.tv_sec * 1000000LL + ts.tv_nsec / 1000; + + int64_t time_us; + if (!use_gps_for_logs || !gps_has_fix) { + time_us = monotonic_us; + } else { + time_us = monotonic_us + monotonic_offset_us; + } + + // Convert to milliseconds and store parts + uint64_t time_ms = time_us / 1000; + last_timestamp_sec = time_ms / 1000; + last_timestamp_ms = time_ms % 1000; + + // Return total milliseconds (ESP-IDF will print this) + // Our custom vprintf will reformat it + return (uint32_t)time_ms; +} + +// Custom vprintf that reformats log timestamps to show decimal point and sync status +// Converts: I (1733424645234) TAG: message +// To: I (+1733424645.234) TAG: message (GPS synced) +// Or: I (*1.234) TAG: message (not synced - monotonic) +int gps_log_vprintf(const char *fmt, va_list args) { + static char buffer[512]; + + // Format the message into our buffer + int ret = vsnprintf(buffer, sizeof(buffer), fmt, args); + + if (use_gps_for_logs) { + // Look for timestamp pattern: "I (", "W (", "E (", etc. + char *timestamp_start = NULL; + for (int i = 0; buffer[i] != '\0' && i < sizeof(buffer) - 20; i++) { + if ((buffer[i] == 'I' || buffer[i] == 'W' || buffer[i] == 'E' || + buffer[i] == 'D' || buffer[i] == 'V') && + buffer[i+1] == ' ' && buffer[i+2] == '(') { + timestamp_start = &buffer[i+3]; + break; + } + } + + if (timestamp_start) { + // Find the closing parenthesis + char *timestamp_end = strchr(timestamp_start, ')'); + if (timestamp_end) { + // Extract timestamp value + uint32_t timestamp_ms = 0; + if (sscanf(timestamp_start, "%lu", ×tamp_ms) == 1) { + uint32_t sec = timestamp_ms / 1000; + uint32_t ms = timestamp_ms % 1000; + + // Choose prefix based on GPS sync status + char prefix = gps_has_fix ? '+' : '*'; + + // Rebuild the string with decimal point and prefix + char reformatted[512]; + size_t prefix_len = timestamp_start - buffer; + + // Copy everything before timestamp + memcpy(reformatted, buffer, prefix_len); + + // Add prefix, formatted timestamp with decimal + int decimal_len = snprintf(reformatted + prefix_len, + sizeof(reformatted) - prefix_len, + "%c%lu.%03u", prefix, sec, ms); + + // Copy everything after timestamp + strcpy(reformatted + prefix_len + decimal_len, timestamp_end); + + // Print the reformatted string + return printf("%s", reformatted); + } + } + } + } + + // If not reformatting or something went wrong, just print original + return printf("%s", buffer); +} diff --git a/components/gps_sync/gps_sync.h b/components/gps_sync/gps_sync.h new file mode 100644 index 0000000..96f972b --- /dev/null +++ b/components/gps_sync/gps_sync.h @@ -0,0 +1,35 @@ +#pragma once + +#include +#include +#include "freertos/FreeRTOS.h" +#include "freertos/semphr.h" + +typedef struct { + int64_t monotonic_us; // Microseconds - never jumps backward + int64_t monotonic_ms; // Milliseconds - for easier logging + int64_t gps_us; // GPS UTC time in microseconds + int64_t gps_ms; // GPS UTC time in milliseconds + struct timespec mono_ts; // POSIX timespec (for clock_gettime) + bool synced; // true if GPS has valid fix +} gps_timestamp_t; + +// Initialize GPS sync system +// If use_gps_log_timestamps is true, ESP_LOGI/ESP_LOGW/etc will use GPS time +// with visual indicators: +// I (+1733424645.234) TAG: message <-- + indicates GPS synced +// I (*1.234) TAG: message <-- * indicates not synced (monotonic) +void gps_sync_init(bool use_gps_log_timestamps); + +// Get current timestamp (with both us and ms) +gps_timestamp_t gps_get_timestamp(void); + +// Get millisecond timestamp using clock_gettime +int64_t gps_get_monotonic_ms(void); + +// Check if GPS is synced +bool gps_is_synced(void); + +// Internal functions (called automatically by ESP-IDF - don't call directly) +uint32_t esp_log_timestamp(void); +int gps_log_vprintf(const char *fmt, va_list args);