481 lines
12 KiB
C
481 lines
12 KiB
C
/*
|
|
* gps_task.c
|
|
*
|
|
* Created on: Jan 20, 2018
|
|
* Author: chris.l
|
|
*/
|
|
|
|
#include "esp_gps_ntp.h"
|
|
|
|
#include "driver/uart.h"
|
|
#include "freertos/timers.h"
|
|
#include "sys/time.h"
|
|
#include "minmea.h"
|
|
|
|
static const char *TAG = "GPS";
|
|
|
|
#define RX_BUF_SIZE 255
|
|
|
|
#define ESP_INTR_FLAG_DEFAULT 0
|
|
|
|
// simple versions - we don't worry about side effects
|
|
#define MAX(a, b) ((a) < (b) ? (b) : (a))
|
|
#define MIN(a, b) ((a) < (b) ? (a) : (b))
|
|
|
|
#define VALIDITY_TIMER_MS 1010
|
|
#define PPS_VALID_COUNT 10 // must have at least this many "good" PPS interrupts to be valid
|
|
|
|
#define PPS_PIN (GPIO_NUM_19)
|
|
#define TXD_PIN (GPIO_NUM_17)
|
|
#define RXD_PIN (GPIO_NUM_16)
|
|
#define LED_PIN (GPIO_NUM_15)
|
|
|
|
#define GPS_UART (UART_NUM_2)
|
|
|
|
static TimerHandle_t timer;
|
|
static volatile uint32_t timeouts;
|
|
static volatile time_t seconds;
|
|
static volatile micros_t last_micros;
|
|
static volatile micros_t min_micros;
|
|
static volatile micros_t max_micros;
|
|
static volatile bool gps_valid;
|
|
static volatile bool valid;
|
|
static volatile uint32_t valid_in;
|
|
static volatile time_t valid_since;
|
|
static volatile uint32_t valid_count;
|
|
static volatile time_t initial_valid;
|
|
static uint32_t dispersion;
|
|
static volatile char reason[128];
|
|
|
|
typedef struct gps_status
|
|
{
|
|
bool valid;
|
|
int sat_count;
|
|
int fix_quality;
|
|
} GPSStatus;
|
|
|
|
static GPSStatus gps_status;
|
|
|
|
|
|
|
|
#ifdef LED_PIN
|
|
#define DEBUG_LED_ON() gpio_set_level(LED_PIN, 1)
|
|
#define DEBUG_LED_OFF() gpio_set_level(LED_PIN, 0)
|
|
#define DEBUG_LED_INIT() init_debug_led()
|
|
static void init_debug_led()
|
|
{
|
|
//
|
|
// set up the LED pin
|
|
//
|
|
|
|
gpio_config_t io_conf;
|
|
//interrupt disabled
|
|
io_conf.intr_type = GPIO_PIN_INTR_DISABLE;
|
|
//set as input mode
|
|
io_conf.mode = GPIO_MODE_OUTPUT;
|
|
//bit mask of the pins that you want to set,e.g.GPIO18/19
|
|
io_conf.pin_bit_mask = (1ULL<<LED_PIN);
|
|
//disable pull-down mode
|
|
io_conf.pull_down_en = 0;
|
|
//disable pull-up mode
|
|
io_conf.pull_up_en = 0;
|
|
//configure GPIO with the given settings
|
|
gpio_config(&io_conf);
|
|
|
|
DEBUG_LED_OFF();
|
|
}
|
|
#else
|
|
#define DEBUG_LED_ON()
|
|
#define DEBUG_LED_OFF()
|
|
#define DEBUG_LED_INIT()
|
|
#endif
|
|
|
|
|
|
uint32_t getDispersion()
|
|
{
|
|
return dispersion;
|
|
}
|
|
|
|
bool isTimeValid()
|
|
{
|
|
return valid;
|
|
}
|
|
|
|
void getTime(time_t *secs, time_t *usecs)
|
|
{
|
|
*secs = seconds;
|
|
*usecs = micros() - last_micros;
|
|
}
|
|
|
|
void getGPSTaskStatus(GPSTaskStatus* status)
|
|
{
|
|
status->timeouts = timeouts;
|
|
status->seconds = seconds;
|
|
status->min_micros = min_micros;
|
|
status->max_micros = max_micros;
|
|
status->gps_valid = gps_valid;
|
|
status->valid = valid;
|
|
status->valid_in = valid_in;
|
|
status->valid_since = valid_since;
|
|
status->valid_count = valid_count;
|
|
status->initial_valid = initial_valid;
|
|
status->sat_count = gps_status.sat_count;
|
|
status->fix_quality = gps_status.fix_quality;
|
|
}
|
|
|
|
static void IRAM_ATTR pps_isr_handler(void* arg)
|
|
{
|
|
DEBUG_LED_ON();
|
|
micros_t cur_micros = micros();
|
|
// restart the validity timer
|
|
xTimerReset(timer, 0);
|
|
|
|
//
|
|
// trigger a display update each second
|
|
//
|
|
triggerDisplay();
|
|
|
|
//
|
|
// don't trust PPS if GPS is not valid.
|
|
//
|
|
if (!gps_valid)
|
|
{
|
|
DEBUG_LED_OFF();
|
|
return;
|
|
}
|
|
|
|
//
|
|
// if we are still counting down then keep waiting
|
|
//
|
|
if (valid_in)
|
|
{
|
|
--valid_in;
|
|
if (valid_in == 0)
|
|
{
|
|
// clear stats and mark us valid
|
|
min_micros = 0;
|
|
max_micros = 0;
|
|
valid = true;
|
|
valid_since = seconds;
|
|
++valid_count;
|
|
reason[0] = '\0';
|
|
if (initial_valid == 0)
|
|
{
|
|
initial_valid = seconds;
|
|
}
|
|
}
|
|
}
|
|
|
|
seconds++;
|
|
//
|
|
// the first time around we just initialize the last value
|
|
//
|
|
if (last_micros == 0)
|
|
{
|
|
last_micros = cur_micros;
|
|
DEBUG_LED_OFF();
|
|
return;
|
|
}
|
|
|
|
uint32_t micros_count = cur_micros - last_micros;
|
|
last_micros = cur_micros;
|
|
|
|
if (min_micros == 0 || micros_count < min_micros)
|
|
{
|
|
min_micros = micros_count;
|
|
}
|
|
|
|
if (micros_count > max_micros)
|
|
{
|
|
max_micros = micros_count;
|
|
}
|
|
|
|
DEBUG_LED_OFF();
|
|
}
|
|
|
|
//
|
|
// read a single NMEA record
|
|
//
|
|
static char *readNMEA()
|
|
{
|
|
static uint8_t line[RX_BUF_SIZE+1];
|
|
int size;
|
|
uint8_t *p = line;
|
|
*p = '\0';
|
|
while(1)
|
|
{
|
|
if (p >= line+RX_BUF_SIZE)
|
|
{
|
|
ESP_LOGI(TAG, "Read buffer overflow! size %d bytes: '%s'", p-line, line);
|
|
}
|
|
size = uart_read_bytes(GPS_UART, (unsigned char *)p, 1, portMAX_DELAY);
|
|
if (size == 1)
|
|
{
|
|
if (*p == '$')
|
|
{
|
|
//
|
|
// '$' starts a record so reset pointer to start
|
|
//
|
|
p = line;
|
|
*p = '$';
|
|
}
|
|
else if (*p == '\r' || line[0] != '$')
|
|
{
|
|
//
|
|
// skip newlin and any chars if buffer does not start with '$'
|
|
continue;
|
|
}
|
|
else if (*p == '\n')
|
|
{
|
|
//
|
|
// got a record!
|
|
//
|
|
*p = 0;
|
|
break;
|
|
}
|
|
p++;
|
|
}
|
|
}
|
|
|
|
return (char*)line;
|
|
}
|
|
|
|
|
|
|
|
static void IRAM_ATTR invalidate(const char* fmt, ...)
|
|
{
|
|
va_list argp;
|
|
va_start(argp, fmt);
|
|
if (reason[0] == '\0')
|
|
{
|
|
vsnprintf((char*)reason, sizeof(reason)-1, fmt, argp);
|
|
}
|
|
va_end(argp);
|
|
|
|
if (valid)
|
|
{
|
|
valid_since = seconds;
|
|
}
|
|
|
|
valid = false;
|
|
gps_valid = false;
|
|
last_micros = 0;
|
|
valid_in = 0;
|
|
}
|
|
|
|
static void init_pps()
|
|
{
|
|
//
|
|
// set up the PPS interrupt
|
|
//
|
|
|
|
gpio_config_t io_conf;
|
|
//interrupt of falling edge
|
|
io_conf.intr_type = GPIO_PIN_INTR_POSEDGE;
|
|
//set as input mode
|
|
io_conf.mode = GPIO_MODE_INPUT;
|
|
//bit mask of the pins that you want to set,e.g.GPIO18/19
|
|
io_conf.pin_bit_mask = (1ULL<<PPS_PIN);
|
|
//disable pull-down mode
|
|
io_conf.pull_down_en = 0;
|
|
//enable pull-up mode
|
|
io_conf.pull_up_en = 1;
|
|
//configure GPIO with the given settings
|
|
gpio_config(&io_conf);
|
|
|
|
//hook isr handler for specific gpio pin
|
|
gpio_isr_handler_add(PPS_PIN, pps_isr_handler, (void*)0);
|
|
}
|
|
|
|
static void init_uart()
|
|
{
|
|
//
|
|
// initialize the uart attached to the GPS module
|
|
//
|
|
const 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
|
|
};
|
|
uart_param_config(GPS_UART, &uart_config);
|
|
uart_set_pin(GPS_UART, TXD_PIN, RXD_PIN, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
|
|
// We won't use a buffer for sending data.
|
|
uart_driver_install(GPS_UART, RX_BUF_SIZE * 2, 0, 0, NULL, 0);
|
|
}
|
|
|
|
static void IRAM_ATTR invalidateTime()
|
|
{
|
|
// only count timeouts after we have been valid once.
|
|
if (initial_valid != 0)
|
|
{
|
|
++timeouts;
|
|
}
|
|
xTimerStart(timer, 0);
|
|
invalidate("TIMEOUT");
|
|
//
|
|
// trigger a display update each timeout
|
|
//
|
|
triggerDisplay();
|
|
}
|
|
|
|
static void init_timer()
|
|
{
|
|
timer = xTimerCreate("MyTimer", pdMS_TO_TICKS(VALIDITY_TIMER_MS), pdFALSE, NULL, &invalidateTime);
|
|
xTimerStart(timer, 0);
|
|
}
|
|
|
|
static void gps_task()
|
|
{
|
|
static bool last_valid = false;
|
|
static bool last_gps_valid = false;
|
|
static int timewarps = 0;
|
|
|
|
DEBUG_LED_INIT();
|
|
init_uart();
|
|
|
|
//install gpio isr service
|
|
gpio_install_isr_service(ESP_INTR_FLAG_DEFAULT);
|
|
|
|
init_pps();
|
|
init_timer();
|
|
|
|
while (1) {
|
|
|
|
//
|
|
// Print valid or invalid if status has changed.
|
|
//
|
|
if ((last_valid && !valid) || (last_gps_valid && !gps_valid))
|
|
{
|
|
ESP_LOGW(TAG, "INVALID: '%s'", reason);
|
|
reason[0] = '\0';
|
|
}
|
|
else if (!last_valid && valid)
|
|
{
|
|
ESP_LOGI(TAG, "VALID!");
|
|
}
|
|
last_valid = valid;
|
|
last_gps_valid = gps_valid;
|
|
|
|
char* line = readNMEA();
|
|
|
|
switch (minmea_sentence_id(line, true))
|
|
{
|
|
case MINMEA_SENTENCE_RMC:
|
|
{
|
|
struct minmea_sentence_rmc frame;
|
|
if (minmea_parse_rmc(&frame, line))
|
|
{
|
|
ESP_LOGD(TAG, "$RMC: seconds: %lu timeouts: %u valid: %s", seconds, timeouts, frame.valid ? "true" : "false");
|
|
gps_status.valid = frame.valid;
|
|
|
|
if (frame.valid)
|
|
{
|
|
struct timespec ts;
|
|
minmea_gettime(&ts, &frame.date, &frame.time);
|
|
if (valid && seconds > ts.tv_sec)
|
|
{
|
|
timewarps += 1;
|
|
ESP_LOGD(TAG, "$RMC: ignoring timewarp back! delayed serial? %lu -> %lu", seconds, ts.tv_sec);
|
|
if (timewarps > 1)
|
|
{
|
|
invalidate("time warped backwards too many (%d) times!", timewarps);
|
|
timewarps = 0;
|
|
}
|
|
}
|
|
else if (seconds != ts.tv_sec)
|
|
{
|
|
ESP_LOGW(TAG, "$RMC: adjusting seconds %lu -> %lu", seconds, ts.tv_sec);
|
|
seconds = ts.tv_sec;
|
|
}
|
|
else
|
|
{
|
|
timewarps = 0;
|
|
}
|
|
|
|
//
|
|
// if gps was not valid, it is now
|
|
//
|
|
if (!gps_valid)
|
|
{
|
|
gps_valid = true;
|
|
valid_in = PPS_VALID_COUNT;
|
|
ESP_LOGI(TAG, "gps valid!");
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (valid)
|
|
{
|
|
invalidate("GPS $RMC");
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
ESP_LOGE(TAG, "$RMC: failed to parse line: %s", line);
|
|
}
|
|
}
|
|
break;
|
|
|
|
case MINMEA_SENTENCE_GGA:
|
|
{
|
|
struct minmea_sentence_gga frame;
|
|
if (minmea_parse_gga(&frame, line)) {
|
|
ESP_LOGD(TAG, "$GGA: seconds: %lu timeouts: %u fix: %d sats: %d", seconds, timeouts, frame.fix_quality, frame.satellites_tracked);
|
|
gps_status.sat_count = frame.satellites_tracked;
|
|
gps_status.fix_quality = frame.fix_quality;
|
|
}
|
|
else
|
|
{
|
|
ESP_LOGE(TAG, "$GGA: failed to parse line: %s", line);
|
|
}
|
|
}
|
|
break;
|
|
|
|
case MINMEA_SENTENCE_GSV:
|
|
case MINMEA_SENTENCE_GSA:
|
|
case MINMEA_SENTENCE_GLL:
|
|
case MINMEA_SENTENCE_GST:
|
|
case MINMEA_SENTENCE_VTG:
|
|
case MINMEA_SENTENCE_ZDA:
|
|
ESP_LOGV(TAG, "IGNORING: %s", line);
|
|
break;
|
|
|
|
case MINMEA_INVALID:
|
|
ESP_LOGE(TAG, "INVALID: '%s'", line);
|
|
break;
|
|
|
|
case MINMEA_UNKNOWN:
|
|
ESP_LOGW(TAG, "UNKNOWN: '%s'", line);
|
|
break;
|
|
}
|
|
|
|
//
|
|
// Recompute dispersion periodically
|
|
//
|
|
static time_t last_seconds;
|
|
if (seconds != last_seconds)
|
|
{
|
|
double disp = us2s(MAX(abs(MICROS_PER_SEC-max_micros), abs(MICROS_PER_SEC-min_micros)));
|
|
dispersion = (uint32_t)(disp * 65536.0);
|
|
#if 0
|
|
ESP_LOGI(TAG, "min: %llu max: %llu jitter: %llu sats: %d fix: %d valid_count: %u valid_in: %u valid: %s",
|
|
min_micros, max_micros, max_micros - min_micros, gps_status.sat_count, gps_status.fix_quality, valid_count, valid_in, valid ? "true" : "false");
|
|
#endif
|
|
}
|
|
last_seconds = seconds;
|
|
|
|
|
|
}
|
|
}
|
|
|
|
void start_gps()
|
|
{
|
|
esp_log_level_set(TAG, CONFIG_GPS_LOG_LEVEL);
|
|
|
|
xTaskCreatePinnedToCore(gps_task, "gps_task", 1024*2, NULL, GPS_TASK_PRI, NULL, 1);
|
|
|
|
}
|