260 lines
8.0 KiB
C
260 lines
8.0 KiB
C
/*
|
|
* gps_sync.c
|
|
*
|
|
* Copyright (c) 2025 Umber Networks & Robert McMahon
|
|
* All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions are met:
|
|
*
|
|
* 1. Redistributions of source code must retain the above copyright notice,
|
|
* this list of conditions and the following disclaimer.
|
|
*
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in the
|
|
* documentation and/or other materials provided with the distribution.
|
|
*
|
|
* 3. Neither the name of the copyright holder nor the names of its
|
|
* contributors may be used to endorse or promote products derived from
|
|
* this software without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
|
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
|
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
|
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
|
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
|
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
|
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
|
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
#include <sys/time.h>
|
|
#include <time.h>
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/task.h"
|
|
#include "esp_log.h"
|
|
#include "esp_timer.h"
|
|
#include "driver/uart.h"
|
|
#include "driver/gpio.h"
|
|
#include "gps_sync.h"
|
|
|
|
static const char *TAG = "GPS_SYNC";
|
|
|
|
#define GPS_BUF_SIZE 1024
|
|
|
|
// --- Internal State ---
|
|
static gps_sync_config_t s_cfg;
|
|
static volatile int64_t s_last_pps_us = 0;
|
|
static volatile int64_t s_nmea_epoch_us = 0;
|
|
static volatile bool s_nmea_valid = false;
|
|
static char s_last_nmea_msg[128] = {0};
|
|
static bool s_time_set = false;
|
|
|
|
// --- PPS Handler ---
|
|
static void IRAM_ATTR pps_gpio_isr_handler(void* arg) {
|
|
s_last_pps_us = esp_timer_get_time();
|
|
}
|
|
|
|
// --- Time Helper ---
|
|
static void set_system_time(char *time_str, char *date_str) {
|
|
// time_str: HHMMSS.ss (e.g., 123519.00)
|
|
// date_str: DDMMYY (e.g., 230394)
|
|
|
|
struct tm tm_info = {0};
|
|
|
|
// Parse Time
|
|
int h, m, s;
|
|
if (sscanf(time_str, "%2d%2d%2d", &h, &m, &s) != 3) return;
|
|
tm_info.tm_hour = h;
|
|
tm_info.tm_min = m;
|
|
tm_info.tm_sec = s;
|
|
|
|
// Parse Date
|
|
int day, mon, year;
|
|
if (sscanf(date_str, "%2d%2d%2d", &day, &mon, &year) != 3) return;
|
|
tm_info.tm_mday = day;
|
|
tm_info.tm_mon = mon - 1; // 0-11
|
|
tm_info.tm_year = year + 100; // Years since 1900 (2025 -> 125)
|
|
|
|
time_t t = mktime(&tm_info);
|
|
if (t == -1) return;
|
|
|
|
struct timeval tv = { .tv_sec = t, .tv_usec = 0 };
|
|
|
|
// Simple sync: Only set if not set, or if drift is massive (>2s)
|
|
// In a real PTP/GPS app you'd use a PLL here, but this is a shell tool.
|
|
struct timeval now;
|
|
gettimeofday(&now, NULL);
|
|
|
|
if (!s_time_set || llabs(now.tv_sec - t) > 2) {
|
|
settimeofday(&tv, NULL);
|
|
s_time_set = true;
|
|
ESP_LOGI(TAG, "System Time Updated to GPS: %s", asctime(&tm_info));
|
|
}
|
|
}
|
|
|
|
// --- NMEA Parser ---
|
|
static void parse_nmea_line(char *line) {
|
|
strlcpy(s_last_nmea_msg, line, sizeof(s_last_nmea_msg));
|
|
|
|
// Support GPRMC and GNRMC
|
|
if (strncmp(line, "$GPRMC", 6) == 0 || strncmp(line, "$GNRMC", 6) == 0) {
|
|
char *p = line;
|
|
int field = 0;
|
|
char *time_ptr = NULL;
|
|
char *date_ptr = NULL;
|
|
char status = 'V';
|
|
|
|
// Walk fields
|
|
// $GPRMC,Time,Status,Lat,NS,Lon,EW,Spd,Trk,Date,...
|
|
// Field 1: Time
|
|
// Field 2: Status
|
|
// Field 9: Date
|
|
|
|
while ((p = strchr(p, ',')) != NULL) {
|
|
p++;
|
|
field++;
|
|
|
|
if (field == 1) time_ptr = p;
|
|
else if (field == 2) status = *p;
|
|
else if (field == 9) {
|
|
date_ptr = p;
|
|
break; // We have what we need
|
|
}
|
|
}
|
|
|
|
s_nmea_valid = (status == 'A');
|
|
|
|
if (s_nmea_valid) {
|
|
s_nmea_epoch_us = esp_timer_get_time();
|
|
|
|
// Extract substrings for Time/Date (comma terminated)
|
|
if (time_ptr && date_ptr) {
|
|
char t_buf[16] = {0};
|
|
char d_buf[16] = {0};
|
|
|
|
char *end = strchr(time_ptr, ',');
|
|
if (end) {
|
|
int len = end - time_ptr;
|
|
if (len < sizeof(t_buf)) {
|
|
memcpy(t_buf, time_ptr, len);
|
|
t_buf[len] = 0;
|
|
}
|
|
}
|
|
|
|
end = strchr(date_ptr, ',');
|
|
if (end) {
|
|
int len = end - date_ptr;
|
|
if (len < sizeof(d_buf)) {
|
|
memcpy(d_buf, date_ptr, len);
|
|
d_buf[len] = 0;
|
|
}
|
|
}
|
|
|
|
// Update System Clock
|
|
if (t_buf[0] && d_buf[0]) {
|
|
set_system_time(t_buf, d_buf);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// --- UART Task ---
|
|
static void gps_task(void *pvParameters) {
|
|
uint8_t *data = (uint8_t *)malloc(GPS_BUF_SIZE);
|
|
if (!data) {
|
|
ESP_LOGE(TAG, "Failed to allocate GPS buffer");
|
|
vTaskDelete(NULL);
|
|
return;
|
|
}
|
|
|
|
char line_buf[128];
|
|
int line_pos = 0;
|
|
|
|
while (1) {
|
|
int len = uart_read_bytes(s_cfg.uart_port, data, GPS_BUF_SIZE, 20 / portTICK_PERIOD_MS);
|
|
if (len > 0) {
|
|
for (int i = 0; i < len; i++) {
|
|
char c = (char)data[i];
|
|
if (c == '\n' || c == '\r') {
|
|
if (line_pos > 0) {
|
|
line_buf[line_pos] = 0;
|
|
parse_nmea_line(line_buf);
|
|
line_pos = 0;
|
|
}
|
|
} else if (line_pos < sizeof(line_buf) - 1) {
|
|
line_buf[line_pos++] = c;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
free(data);
|
|
vTaskDelete(NULL);
|
|
}
|
|
|
|
// --- API ---
|
|
|
|
void gps_sync_init(const gps_sync_config_t *cfg, bool force_enable) {
|
|
if (!cfg) return;
|
|
s_cfg = *cfg;
|
|
|
|
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_DISABLE,
|
|
.source_clk = UART_SCLK_DEFAULT,
|
|
};
|
|
uart_driver_install(s_cfg.uart_port, GPS_BUF_SIZE * 2, 0, 0, NULL, 0);
|
|
uart_param_config(s_cfg.uart_port, &uart_config);
|
|
uart_set_pin(s_cfg.uart_port, s_cfg.tx_pin, s_cfg.rx_pin, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
|
|
|
|
gpio_config_t io_conf = {};
|
|
io_conf.intr_type = GPIO_INTR_POSEDGE;
|
|
io_conf.pin_bit_mask = (1ULL << s_cfg.pps_pin);
|
|
io_conf.mode = GPIO_MODE_INPUT;
|
|
io_conf.pull_up_en = 1;
|
|
gpio_config(&io_conf);
|
|
|
|
gpio_install_isr_service(0);
|
|
gpio_isr_handler_add(s_cfg.pps_pin, pps_gpio_isr_handler, NULL);
|
|
|
|
xTaskCreate(gps_task, "gps_task", 4096, NULL, 5, NULL);
|
|
|
|
ESP_LOGI(TAG, "Initialized (UART:%d, PPS:%d)", s_cfg.uart_port, s_cfg.pps_pin);
|
|
}
|
|
|
|
gps_timestamp_t gps_get_timestamp(void) {
|
|
gps_timestamp_t ts = {0};
|
|
int64_t now_boot = esp_timer_get_time(); // Boot time
|
|
|
|
// Check Flags
|
|
ts.synced = (now_boot - s_last_pps_us < 1100000);
|
|
ts.valid = s_nmea_valid && (now_boot - s_nmea_epoch_us < 2000000);
|
|
|
|
// Return WALL CLOCK time (Epoch), not boot time
|
|
struct timeval tv;
|
|
gettimeofday(&tv, NULL);
|
|
ts.gps_us = (int64_t)tv.tv_sec * 1000000LL + (int64_t)tv.tv_usec;
|
|
|
|
return ts;
|
|
}
|
|
|
|
int64_t gps_get_pps_age_ms(void) {
|
|
if (s_last_pps_us == 0) return -1;
|
|
return (esp_timer_get_time() - s_last_pps_us) / 1000;
|
|
}
|
|
|
|
void gps_get_last_nmea(char *buf, size_t buf_len) {
|
|
if (buf && buf_len > 0) {
|
|
strlcpy(buf, s_last_nmea_msg, buf_len);
|
|
}
|
|
}
|