Umber
/
ESP32
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add gps

This commit is contained in:
Bob 2025-12-06 11:48:03 -08:00
parent 20aab3d6e7
commit 283d524fc5
3 changed files with 350 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
components/gps_sync/CMakeLists.txt Normal file
View File

@ -0,0 +1,5 @@
idf_component_register(
SRCS "gps_sync.c"
INCLUDE_DIRS "include"
REQUIRES driver
)

310
components/gps_sync/gps_sync.c Normal file
View File

@ -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 <string.h>
#include <time.h>
#include <stdarg.h>
#include <stdio.h>
#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", &timestamp_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);
}

35
components/gps_sync/gps_sync.h Normal file
View File

@ -0,0 +1,35 @@
#pragma once
#include <stdint.h>
#include <time.h>
#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);