388 lines
13 KiB
C
388 lines
13 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_err.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,
|
|
};
|
|
esp_err_t err = uart_driver_install(s_cfg.uart_port, GPS_BUF_SIZE * 2, 0, 0, NULL, 0);
|
|
if (err != ESP_OK && err != ESP_ERR_INVALID_STATE) {
|
|
ESP_LOGE(TAG, "Failed to install UART driver: %s", esp_err_to_name(err));
|
|
return;
|
|
}
|
|
uart_param_config(s_cfg.uart_port, &uart_config);
|
|
err = uart_set_pin(s_cfg.uart_port, s_cfg.tx_pin, s_cfg.rx_pin, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
|
|
if (err != ESP_OK) {
|
|
ESP_LOGE(TAG, "Failed to set UART pins: %s", esp_err_to_name(err));
|
|
return;
|
|
}
|
|
|
|
gpio_config_t io_conf = {};
|
|
io_conf.intr_type = s_cfg.pps_active_low ? GPIO_INTR_NEGEDGE : 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;
|
|
err = gpio_config(&io_conf);
|
|
if (err != ESP_OK) {
|
|
ESP_LOGE(TAG, "Failed to configure PPS GPIO %d: %s", s_cfg.pps_pin, esp_err_to_name(err));
|
|
return;
|
|
}
|
|
|
|
// Install ISR service (ignore error if already installed)
|
|
err = gpio_install_isr_service(0);
|
|
if (err != ESP_OK && err != ESP_ERR_INVALID_STATE) {
|
|
ESP_LOGE(TAG, "Failed to install GPIO ISR service: %s", esp_err_to_name(err));
|
|
return;
|
|
}
|
|
|
|
err = gpio_isr_handler_add(s_cfg.pps_pin, pps_gpio_isr_handler, NULL);
|
|
if (err != ESP_OK) {
|
|
ESP_LOGE(TAG, "Failed to add PPS GPIO ISR handler: %s", esp_err_to_name(err));
|
|
return;
|
|
}
|
|
|
|
xTaskCreate(gps_task, "gps_task", 4096, NULL, 5, NULL);
|
|
|
|
ESP_LOGI(TAG, "Initialized (UART:%d, PPS:%d %s)", s_cfg.uart_port, s_cfg.pps_pin,
|
|
s_cfg.pps_active_low ? "falling-edge" : "rising-edge");
|
|
}
|
|
|
|
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);
|
|
}
|
|
}
|
|
|
|
void gps_pps_diagnostic(int duration_sec) {
|
|
if (duration_sec < 1) duration_sec = 1;
|
|
if (duration_sec > 30) duration_sec = 30;
|
|
|
|
const int interval_ms = 50;
|
|
int samples = (duration_sec * 1000) / interval_ms;
|
|
int prev_level = -1;
|
|
int rising = 0, falling = 0;
|
|
|
|
printf("PPS diagnostic: polling GPIO%d for %d sec (interval %d ms)...\n",
|
|
s_cfg.pps_pin, duration_sec, interval_ms);
|
|
|
|
for (int i = 0; i < samples; i++) {
|
|
int level = gpio_get_level(s_cfg.pps_pin);
|
|
if (prev_level >= 0) {
|
|
if (level > prev_level) rising++;
|
|
else if (level < prev_level) falling++;
|
|
}
|
|
prev_level = level;
|
|
vTaskDelay(pdMS_TO_TICKS(interval_ms));
|
|
}
|
|
|
|
int total = rising + falling;
|
|
printf(" Samples: %d, Rising edges: %d, Falling edges: %d\n", samples, rising, falling);
|
|
/* 1 Hz pulse gives ~2 edges/sec (rising+falling) */
|
|
int lo = 2 * (duration_sec - 1);
|
|
int hi = 2 * (duration_sec + 1);
|
|
if (total >= lo && total <= hi) {
|
|
printf(" Signal: DETECTED (~1 Hz)\n");
|
|
printf(" Try opposite polarity: set GPS_PPS_ACTIVE_LOW=%d in board_config.h\n",
|
|
falling > rising ? 0 : 1);
|
|
} else if (total == 0) {
|
|
printf(" Signal: NOT DETECTED (no edges)\n");
|
|
printf(" Check: PPS wire, GPS PPS pin, UBX-CFG-TP to enable timepulse\n");
|
|
} else {
|
|
printf(" Signal: UNCLEAR (%d edges in %ds, expected ~%d)\n", total, duration_sec, 2 * duration_sec);
|
|
}
|
|
}
|
|
|
|
// Candidate PPS pins for ESP32-C5 (J1: 6=GPIO1, 7=GPIO6, 13=GPIO25)
|
|
static const int s_pps_scan_pins[] = { 1, 6, 25 };
|
|
#define PPS_SCAN_PIN_COUNT (sizeof(s_pps_scan_pins) / sizeof(s_pps_scan_pins[0]))
|
|
|
|
static void poll_pin_edges(int pin, int duration_sec, int interval_ms,
|
|
int *rising, int *falling) {
|
|
gpio_config_t io = {
|
|
.pin_bit_mask = (1ULL << pin),
|
|
.mode = GPIO_MODE_INPUT,
|
|
.pull_up_en = 1,
|
|
.pull_down_en = 0,
|
|
.intr_type = GPIO_INTR_DISABLE,
|
|
};
|
|
gpio_config(&io);
|
|
|
|
int samples = (duration_sec * 1000) / interval_ms;
|
|
int prev = -1;
|
|
*rising = *falling = 0;
|
|
for (int i = 0; i < samples; i++) {
|
|
int lvl = gpio_get_level(pin);
|
|
if (prev >= 0) {
|
|
if (lvl > prev) (*rising)++;
|
|
else if (lvl < prev) (*falling)++;
|
|
}
|
|
prev = lvl;
|
|
vTaskDelay(pdMS_TO_TICKS(interval_ms));
|
|
}
|
|
}
|
|
|
|
void gps_pps_scan(int duration_sec) {
|
|
if (duration_sec < 1) duration_sec = 1;
|
|
if (duration_sec > 10) duration_sec = 10;
|
|
const int interval_ms = 50;
|
|
|
|
printf("PPS scan: polling GPIO 1, 6, 25 for %d sec each...\n", duration_sec);
|
|
printf("(J1 Pin 6=GPIO1, Pin 7=GPIO6, Pin 13=GPIO25)\n\n");
|
|
|
|
for (size_t i = 0; i < PPS_SCAN_PIN_COUNT; i++) {
|
|
int pin = s_pps_scan_pins[i];
|
|
int rising, falling;
|
|
poll_pin_edges(pin, duration_sec, interval_ms, &rising, &falling);
|
|
int total = rising + falling;
|
|
/* 1 Hz pulse gives ~2 edges/sec (rising+falling) */
|
|
int lo = 2 * (duration_sec - 1);
|
|
int hi = 2 * (duration_sec + 1);
|
|
|
|
const char *result;
|
|
if (total >= lo && total <= hi) {
|
|
result = "DETECTED (~1 Hz)";
|
|
} else if (total == 0) {
|
|
result = "no signal";
|
|
} else {
|
|
result = "unclear";
|
|
}
|
|
|
|
printf(" GPIO%-2d (J1 P%2d): rising=%d falling=%d -> %s\n",
|
|
pin, (pin == 1) ? 6 : (pin == 6) ? 7 : 13, rising, falling, result);
|
|
|
|
if (total >= lo && total <= hi) {
|
|
printf(" Set GPS_PPS_PIN to GPIO_NUM_%d in board_config.h\n", pin);
|
|
printf(" Polarity: set GPS_PPS_ACTIVE_LOW=%d\n", falling > rising ? 1 : 0);
|
|
}
|
|
}
|
|
}
|