365 lines
14 KiB
C
365 lines
14 KiB
C
#include "gps_sync.h"
|
|
#include "driver/gpio.h"
|
|
#include "driver/uart.h"
|
|
#include "esp_timer.h"
|
|
#include "esp_log.h"
|
|
#include "esp_rom_sys.h"
|
|
#include <string.h>
|
|
#include <time.h>
|
|
#include <stdarg.h>
|
|
#include <stdio.h>
|
|
#include <assert.h>
|
|
#include <inttypes.h>
|
|
|
|
static const char *TAG = "GPS_SYNC";
|
|
|
|
#define GPS_BAUD_RATE 9600
|
|
#define UART_BUF_SIZE 1024
|
|
|
|
// --- GLOBAL STATE ---
|
|
static uart_port_t gps_uart_num = UART_NUM_1;
|
|
static int64_t monotonic_offset_us = 0;
|
|
static volatile int64_t last_pps_monotonic = 0;
|
|
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;
|
|
|
|
// PPS interrupt
|
|
// Stores the monotonic time of the rising edge of the PPS signal
|
|
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_isdst = 0;
|
|
|
|
return true;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
static void gps_task(void* arg) {
|
|
uint8_t d_buf[64];
|
|
char line[128];
|
|
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';
|
|
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 now = esp_timer_get_time();
|
|
int64_t age_us = now - last_pps;
|
|
|
|
// 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) {
|
|
|
|
// 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;
|
|
|
|
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
|
|
}
|
|
} else {
|
|
// Exponential Smoothing (Filter Jitter)
|
|
// 90% old, 10% new
|
|
monotonic_offset_us = (monotonic_offset_us * 9 + new_offset) / 10;
|
|
}
|
|
gps_has_fix = true;
|
|
xSemaphoreGive(sync_mutex);
|
|
|
|
// Periodic Logging
|
|
if (log_counter <= 0) {
|
|
ESP_LOGI(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--;
|
|
|
|
} else {
|
|
// PPS signal lost or not correlated
|
|
if (log_counter <= 0) {
|
|
ESP_LOGW(TAG, "GPS valid but PPS missing/old (Age: %" PRIi64 " ms)", age_us / 1000);
|
|
log_counter = 10;
|
|
}
|
|
log_counter--;
|
|
}
|
|
} else {
|
|
gps_has_fix = false;
|
|
}
|
|
}
|
|
}
|
|
pos = 0;
|
|
} else {
|
|
if (pos < sizeof(line) - 1) {
|
|
line[pos++] = data;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
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,
|
|
.pull_up_en = GPIO_PULLUP_DISABLE,
|
|
.pull_down_en = GPIO_PULLDOWN_DISABLE,
|
|
.intr_type = GPIO_INTR_DISABLE
|
|
};
|
|
ESP_ERROR_CHECK(gpio_config(&pps_poll_conf));
|
|
|
|
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;
|
|
break;
|
|
}
|
|
vTaskDelay(pdMS_TO_TICKS(1));
|
|
}
|
|
|
|
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();
|
|
sync_mutex = xSemaphoreCreateMutex();
|
|
|
|
if (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,
|
|
.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(config->uart_port, UART_BUF_SIZE, 0, 0, NULL, 0));
|
|
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_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);
|
|
}
|
|
|
|
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;
|
|
xSemaphoreGive(sync_mutex);
|
|
|
|
ts.gps_ms = ts.gps_us / 1000;
|
|
return ts;
|
|
}
|
|
|
|
int64_t gps_get_monotonic_ms(void) {
|
|
return esp_timer_get_time() / 1000;
|
|
}
|
|
|
|
bool gps_is_synced(void) {
|
|
return gps_has_fix;
|
|
}
|
|
|
|
// ---------------- LOGGING SYSTEM INTERCEPTION ----------------
|
|
|
|
uint32_t gps_log_timestamp(void) {
|
|
return (uint32_t)(esp_timer_get_time() / 1000ULL);
|
|
}
|
|
|
|
int gps_log_vprintf(const char *fmt, va_list args) {
|
|
static char buffer[512];
|
|
int ret = vsnprintf(buffer, sizeof(buffer), fmt, args);
|
|
assert(ret >= 0);
|
|
|
|
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') &&
|
|
buffer[i+1] == ' ' && buffer[i+2] == '(') {
|
|
timestamp_start = &buffer[i+3];
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (timestamp_start) {
|
|
char *timestamp_end = strchr(timestamp_start, ')');
|
|
if (timestamp_end) {
|
|
uint32_t monotonic_log_ms = 0;
|
|
if (sscanf(timestamp_start, "%lu", &monotonic_log_ms) == 1) {
|
|
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);
|
|
|
|
int decimal_len = 0;
|
|
|
|
if (gps_has_fix) {
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return printf("%s", buffer);
|
|
}
|