137 lines
3.7 KiB
C
137 lines
3.7 KiB
C
#include "driver/uart.h"
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/task.h"
|
|
#include "esp_log.h"
|
|
#include "string.h"
|
|
#include "gps.h"
|
|
|
|
#define TAG "GPS"
|
|
#define BUF_SIZE 1024
|
|
#define GPS_LINE_MAX_LEN 128
|
|
|
|
static QueueHandle_t uart_queue;
|
|
|
|
// Initializes UART for GPS
|
|
void gps_init()
|
|
{
|
|
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_CTS_RTS,
|
|
.rx_flow_ctrl_thresh = 122,
|
|
};
|
|
ESP_ERROR_CHECK(uart_param_config(GPS_UART_NUM, &uart_config));
|
|
ESP_ERROR_CHECK(uart_set_pin(GPS_UART_NUM, GPS_TXD, GPS_RXD, GPS_RTS, GPS_CTS));
|
|
|
|
const int uart_buffer_size = (1024 * 2);
|
|
ESP_ERROR_CHECK(uart_driver_install(GPS_UART_NUM, uart_buffer_size, uart_buffer_size, 10, &uart_queue, 0));
|
|
}
|
|
|
|
|
|
void gps_task(void *arg)
|
|
{
|
|
uint8_t byte;
|
|
char line[GPS_LINE_MAX_LEN];
|
|
size_t line_pos = 0;
|
|
|
|
gps_init();
|
|
|
|
while (1) {
|
|
int len = uart_read_bytes(GPS_UART_NUM, &byte, 1, pdMS_TO_TICKS(1000));
|
|
if (len > 0) {
|
|
if (byte == '\n') {
|
|
line[line_pos] = '\0';
|
|
|
|
if (line[0] == '$') {
|
|
ESP_LOGV(TAG, "Received NMEA: %s", line);
|
|
|
|
if (strstr(line, "$GPGGA") == line) {
|
|
parse_gpgga(line);
|
|
} else if (strstr(line, "$GPRMC") == line) {
|
|
parse_gprmc(line);
|
|
}
|
|
}
|
|
|
|
line_pos = 0;
|
|
} else if (byte != '\r' && line_pos < (GPS_LINE_MAX_LEN - 1)) {
|
|
line[line_pos++] = byte;
|
|
} else if (line_pos >= GPS_LINE_MAX_LEN - 1) {
|
|
ESP_LOGW(TAG, "Line too long, discarded");
|
|
line_pos = 0;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void parse_gpgga(const char *nmea)
|
|
{
|
|
char *fields[15];
|
|
char temp[GPS_LINE_MAX_LEN];
|
|
strncpy(temp, nmea, GPS_LINE_MAX_LEN);
|
|
temp[GPS_LINE_MAX_LEN - 1] = '\0';
|
|
|
|
int i = 0;
|
|
char *token = strtok(temp, ",");
|
|
while (token != NULL && i < 15) {
|
|
fields[i++] = token;
|
|
token = strtok(NULL, ",");
|
|
}
|
|
|
|
if (i < 10) return; // Not enough fields
|
|
|
|
// Time (hhmmss.sss)
|
|
const char *utc_time = fields[1];
|
|
|
|
// Latitude (ddmm.mmmm)
|
|
const char *lat = fields[2];
|
|
const char *lat_dir = fields[3];
|
|
|
|
// Longitude (dddmm.mmmm)
|
|
const char *lon = fields[4];
|
|
const char *lon_dir = fields[5];
|
|
|
|
// Fix quality (0 = invalid, 1 = GPS fix, 2 = DGPS fix)
|
|
const char *fix_quality = fields[6];
|
|
|
|
// Number of satellites
|
|
const char *num_satellites = fields[7];
|
|
|
|
// Altitude
|
|
const char *altitude = fields[9];
|
|
|
|
ESP_LOGI(TAG, "[GPGGA] Time: %s, Lat: %s %s, Lon: %s %s, Fix: %s, Sats: %s, Alt: %sm",
|
|
utc_time, lat, lat_dir, lon, lon_dir, fix_quality, num_satellites, altitude);
|
|
}
|
|
|
|
void parse_gprmc(const char *nmea)
|
|
{
|
|
char *fields[13];
|
|
char temp[GPS_LINE_MAX_LEN];
|
|
strncpy(temp, nmea, GPS_LINE_MAX_LEN);
|
|
temp[GPS_LINE_MAX_LEN - 1] = '\0';
|
|
|
|
int i = 0;
|
|
char *token = strtok(temp, ",");
|
|
while (token != NULL && i < 13) {
|
|
fields[i++] = token;
|
|
token = strtok(NULL, ",");
|
|
}
|
|
|
|
if (i < 12) return;
|
|
|
|
const char *utc_time = fields[1];
|
|
const char *status = fields[2]; // A = active, V = void
|
|
const char *lat = fields[3];
|
|
const char *lat_dir = fields[4];
|
|
const char *lon = fields[5];
|
|
const char *lon_dir = fields[6];
|
|
const char *speed_knots = fields[7];
|
|
const char *date = fields[9];
|
|
|
|
ESP_LOGI(TAG, "[GPRMC] Date: %s, Time: %s, Lat: %s %s, Lon: %s %s, Speed: %s knots, Status: %s",
|
|
date, utc_time, lat, lat_dir, lon, lon_dir, speed_knots, status);
|
|
}
|
|
|