diff --git a/components/gps_sync/gps_sync.c b/components/gps_sync/gps_sync.c index 915a521..00954a4 100644 --- a/components/gps_sync/gps_sync.c +++ b/components/gps_sync/gps_sync.c @@ -24,77 +24,59 @@ static bool gps_has_fix = false; static bool use_gps_for_logs = false; static SemaphoreHandle_t sync_mutex; static volatile bool force_sync_update = true; + +// Debug Buffer static char s_last_nmea_line[128] = ""; -// PPS interrupt -// Stores the monotonic time of the rising edge of the PPS signal +// PPS interrupt: Captures the exact monotonic time of the rising edge static void IRAM_ATTR pps_isr_handler(void* arg) { - // Capture time immediately int64_t now = esp_timer_get_time(); last_pps_monotonic = now; } // Parse GPS time from NMEA (GPRMC or GNRMC) static bool parse_gprmc(const char* nmea, struct tm* tm_out, bool* valid) { - // Check Header if (strncmp(nmea, "$GPRMC", 6) != 0 && strncmp(nmea, "$GNRMC", 6) != 0) return false; - // Find Time Field char *p = strchr(nmea, ','); if (!p) return false; p++; // Move past comma int hour, min, sec; - // Scan %2d%2d%2d effectively, using floats can be safer for sub-seconds but int is fine for PPS if (sscanf(p, "%2d%2d%2d", &hour, &min, &sec) != 3) return false; - // Find Status Field (A=Active/Valid, V=Void) p = strchr(p, ','); if (!p) return false; p++; *valid = (*p == 'A'); - // Skip Latitude, N/S, Longitude, E/W, Speed, Course (6 fields) for (int i = 0; i < 7; i++) { p = strchr(p, ','); if (!p) return false; p++; } - // Date Field int day, month, year; if (sscanf(p, "%2d%2d%2d", &day, &month, &year) != 3) return false; - // Adjust Year (NMEA provides 2 digits) 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_mon is 0-11 - tm_out->tm_year = year - 1900; // tm_year is years since 1900 + tm_out->tm_mon = month - 1; + tm_out->tm_year = year - 1900; tm_out->tm_isdst = 0; return true; } -// --- PPS Age Getter --- -int64_t gps_get_pps_age_ms(void) { - if (last_pps_monotonic == 0) - return -1; // Never seen - - int64_t now = esp_timer_get_time(); - return (now - last_pps_monotonic) / 1000; -} - void gps_force_next_update(void) { force_sync_update = true; ESP_LOGW(TAG, "Requesting forced GPS sync update"); } -// Helper to convert struct tm to time_t (UTC assumption) -// Uses standard mktime but we assume TZ is handled or default is UTC static time_t timegm_impl(struct tm *tm) { time_t t = mktime(tm); return t; @@ -106,24 +88,21 @@ static void gps_task(void* arg) { int pos = 0; static int log_counter = 0; - // Ensure timezone is UTC for correct time math setenv("TZ", "UTC", 1); tzset(); while (1) { - // Read from UART with a reasonable timeout int len = uart_read_bytes(gps_uart_num, d_buf, sizeof(d_buf), pdMS_TO_TICKS(100)); if (len > 0) { for (int i = 0; i < len; i++) { uint8_t data = d_buf[i]; - // Buffer the line if (data == '\n' || data == '\r') { if (pos > 0) { line[pos] = '\0'; - // Read NMEA string over UART + // Copy to debug buffer xSemaphoreTake(sync_mutex, portMAX_DELAY); strncpy(s_last_nmea_line, line, sizeof(s_last_nmea_line) - 1); s_last_nmea_line[sizeof(s_last_nmea_line) - 1] = '\0'; @@ -132,46 +111,27 @@ static void gps_task(void* arg) { struct tm gps_tm; bool valid_fix; - // Try to parse GPRMC if (parse_gprmc(line, &gps_tm, &valid_fix)) { if (valid_fix) { - // 1. Convert Parsed Time to Seconds time_t gps_time_sec = timegm_impl(&gps_tm); - - // 2. Critical Section: Read PPS Timestamp - xSemaphoreTake(sync_mutex, portMAX_DELAY); - int64_t last_pps = last_pps_monotonic; - xSemaphoreGive(sync_mutex); - - // 3. Analyze Timing + int64_t last_pps_snap = last_pps_monotonic; int64_t now = esp_timer_get_time(); - int64_t age_us = now - last_pps; + int64_t age_us = now - last_pps_snap; - // The PPS pulse described by this message should have happened - // fairly recently (e.g., within the last 800ms). - // If age > 900ms, we likely missed the pulse or UART is lagging badly. - if (last_pps > 0 && age_us < 900000) { + if (last_pps_snap > 0 && age_us < 900000) { - // Calculate Offset - // GPS Time (in microseconds) = Seconds * 1M int64_t gps_time_us = (int64_t)gps_time_sec * 1000000LL; - - // Offset = GPS_Timestamp - Monotonic_Timestamp - // This means: GPS = Monotonic + Offset - int64_t new_offset = gps_time_us - last_pps; + int64_t new_offset = gps_time_us - last_pps_snap; xSemaphoreTake(sync_mutex, portMAX_DELAY); if (monotonic_offset_us == 0 || force_sync_update) { - // Hard Snap (First fix or Forced) monotonic_offset_us = new_offset; if (force_sync_update) { ESP_LOGW(TAG, "GPS SNAP: Offset forced to %" PRIi64 " us", monotonic_offset_us); force_sync_update = false; - log_counter = 0; // Force immediate log + log_counter = 0; } } else { - // Exponential Smoothing (Filter Jitter) - // 90% old, 10% new monotonic_offset_us = (monotonic_offset_us * 9 + new_offset) / 10; } gps_has_fix = true; @@ -179,15 +139,16 @@ static void gps_task(void* arg) { // Periodic Logging if (log_counter <= 0) { - ESP_LOGI(TAG, "GPS Sync: %02d:%02d:%02d | Offset: %" PRIi64 " us | PPS Age: %" PRIi64 " ms", + // CHANGED FROM ESP_LOGI TO ESP_LOGD (Hidden by default) + ESP_LOGD(TAG, "GPS Sync: %02d:%02d:%02d | Offset: %" PRIi64 " us | PPS Age: %" PRIi64 " ms", gps_tm.tm_hour, gps_tm.tm_min, gps_tm.tm_sec, monotonic_offset_us, age_us / 1000); - log_counter = 10; // Log every 10 valid fixes + log_counter = 10; } log_counter--; } else { - // PPS signal lost or not correlated + // Keep Warnings visible if (log_counter <= 0) { ESP_LOGW(TAG, "GPS valid but PPS missing/old (Age: %" PRIi64 " ms)", age_us / 1000); log_counter = 10; @@ -210,11 +171,17 @@ static void gps_task(void* arg) { } } +void gps_get_last_nmea(char *buf, size_t max_len) { + if (!buf || max_len == 0) return; + xSemaphoreTake(sync_mutex, portMAX_DELAY); + strncpy(buf, s_last_nmea_line, max_len); + buf[max_len - 1] = '\0'; + xSemaphoreGive(sync_mutex); +} + void gps_sync_init(const gps_sync_config_t *config, bool use_gps_log_timestamps) { ESP_LOGI(TAG, "Initializing GPS Sync (UART %d, PPS GPIO %d)", config->uart_port, config->pps_pin); - // 1. Initial PPS Pin Check (Input Mode) - // We poll briefly just to see if the pin is physically toggling before committing resources. gpio_config_t pps_poll_conf = { .pin_bit_mask = (1ULL << config->pps_pin), .mode = GPIO_MODE_INPUT, @@ -226,7 +193,6 @@ void gps_sync_init(const gps_sync_config_t *config, bool use_gps_log_timestamps) bool pps_detected = false; int start_level = gpio_get_level(config->pps_pin); - // Poll for up to 2 seconds for (int i = 0; i < 2000; i++) { if (gpio_get_level(config->pps_pin) != start_level) { pps_detected = true; @@ -237,12 +203,10 @@ void gps_sync_init(const gps_sync_config_t *config, bool use_gps_log_timestamps) if (!pps_detected) { ESP_LOGW(TAG, "No PPS signal detected on GPIO %d during boot check.", config->pps_pin); - // We continue anyway, as GPS might gain lock later. } else { ESP_LOGI(TAG, "PPS signal activity detected."); } - // 2. Setup Globals gps_uart_num = config->uart_port; use_gps_for_logs = use_gps_log_timestamps; gps_force_next_update(); @@ -252,7 +216,6 @@ void gps_sync_init(const gps_sync_config_t *config, bool use_gps_log_timestamps) esp_log_set_vprintf(gps_log_vprintf); } - // 3. Setup UART uart_config_t uart_config = { .baud_rate = GPS_BAUD_RATE, .data_bits = UART_DATA_8_BITS, @@ -266,43 +229,28 @@ void gps_sync_init(const gps_sync_config_t *config, bool use_gps_log_timestamps) ESP_ERROR_CHECK(uart_param_config(config->uart_port, &uart_config)); ESP_ERROR_CHECK(uart_set_pin(config->uart_port, config->tx_pin, config->rx_pin, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE)); - // 4. Setup PPS Interrupt (Rising Edge) gpio_config_t pps_intr_conf = { .intr_type = GPIO_INTR_POSEDGE, .mode = GPIO_MODE_INPUT, .pin_bit_mask = (1ULL << config->pps_pin), - .pull_up_en = GPIO_PULLUP_DISABLE, // High-Z usually best for driven PPS + .pull_up_en = GPIO_PULLUP_DISABLE, .pull_down_en = GPIO_PULLDOWN_DISABLE, }; ESP_ERROR_CHECK(gpio_config(&pps_intr_conf)); - // Install ISR service if not already done by other components - // Note: If you have other components using GPIO ISRs, ensure flags match or install is called only once. gpio_install_isr_service(0); ESP_ERROR_CHECK(gpio_isr_handler_add(config->pps_pin, pps_isr_handler, NULL)); - // 5. Start Processor Task xTaskCreate(gps_task, "gps_task", 4096, NULL, 5, NULL); } -void gps_get_last_nmea(char *buf, size_t max_len) { - if (!buf || max_len == 0) return; - xSemaphoreTake(sync_mutex, portMAX_DELAY); - strncpy(buf, s_last_nmea_line, max_len); - buf[max_len - 1] = '\0'; - xSemaphoreGive(sync_mutex); -} - gps_timestamp_t gps_get_timestamp(void) { gps_timestamp_t ts; - // Capture raw monotonic time clock_gettime(CLOCK_MONOTONIC, &ts.mono_ts); - // Calculate microseconds ts.monotonic_us = (int64_t)ts.mono_ts.tv_sec * 1000000LL + ts.mono_ts.tv_nsec / 1000; ts.monotonic_ms = ts.monotonic_us / 1000; - // Apply Offset safely xSemaphoreTake(sync_mutex, portMAX_DELAY); ts.gps_us = ts.monotonic_us + monotonic_offset_us; ts.synced = gps_has_fix; @@ -320,6 +268,13 @@ bool gps_is_synced(void) { return gps_has_fix; } +// --- NEW: PPS Age Getter --- +int64_t gps_get_pps_age_ms(void) { + if (last_pps_monotonic == 0) return -1; + int64_t now = esp_timer_get_time(); + return (now - last_pps_monotonic) / 1000; +} + // ---------------- LOGGING SYSTEM INTERCEPTION ---------------- uint32_t gps_log_timestamp(void) { @@ -333,7 +288,6 @@ int gps_log_vprintf(const char *fmt, va_list args) { if (use_gps_for_logs) { char *timestamp_start = NULL; - // Find ESP log timestamp format "(1234)" 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') && @@ -351,7 +305,6 @@ int gps_log_vprintf(const char *fmt, va_list args) { char reformatted[512]; size_t prefix_len = timestamp_start - buffer; - // Copy prefix "I " if (prefix_len > sizeof(reformatted)) prefix_len = sizeof(reformatted); memcpy(reformatted, buffer, prefix_len); @@ -361,24 +314,19 @@ int gps_log_vprintf(const char *fmt, va_list args) { int64_t log_mono_us = (int64_t)monotonic_log_ms * 1000; int64_t log_gps_us = log_mono_us + monotonic_offset_us; - // Split into seconds and fractional ms uint64_t gps_sec = log_gps_us / 1000000; uint32_t gps_ms = (log_gps_us % 1000000) / 1000; - // Overwrite timestamp with GPS time e.g., "+1703000000.123" decimal_len = snprintf(reformatted + prefix_len, sizeof(reformatted) - prefix_len, "+%" PRIu64 ".%03lu", gps_sec, gps_ms); } else { - // Fallback: Keep monotonic but mark as unsynced uint32_t sec = monotonic_log_ms / 1000; uint32_t ms = monotonic_log_ms % 1000; decimal_len = snprintf(reformatted + prefix_len, sizeof(reformatted) - prefix_len, "*%lu.%03lu", (unsigned long)sec, (unsigned long)ms); } - - // Copy remainder of message strcpy(reformatted + prefix_len + decimal_len, timestamp_end); return printf("%s", reformatted); }