2018-11-28 05:49:18 -05:00
|
|
|
// Copyright 2015-2018 Espressif Systems (Shanghai) PTE LTD
|
|
|
|
|
//
|
|
|
|
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
|
|
|
|
// you may not use this file except in compliance with the License.
|
|
|
|
|
// You may obtain a copy of the License at
|
|
|
|
|
//
|
|
|
|
|
// http://www.apache.org/licenses/LICENSE-2.0
|
|
|
|
|
//
|
|
|
|
|
// Unless required by applicable law or agreed to in writing, software
|
|
|
|
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
|
|
|
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
|
|
|
// See the License for the specific language governing permissions and
|
|
|
|
|
// limitations under the License.
|
|
|
|
|
|
|
|
|
|
#include <stdlib.h>
|
|
|
|
|
#include <string.h>
|
|
|
|
|
#include <ctype.h>
|
|
|
|
|
#include <math.h>
|
|
|
|
|
#include "freertos/FreeRTOS.h"
|
|
|
|
|
#include "freertos/task.h"
|
|
|
|
|
#include "esp_log.h"
|
|
|
|
|
#include "nmea_parser.h"
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* @brief NMEA Parser runtime buffer size
|
|
|
|
|
*
|
|
|
|
|
*/
|
|
|
|
|
#define NMEA_PARSER_RUNTIME_BUFFER_SIZE (CONFIG_NMEA_PARSER_RING_BUFFER_SIZE / 2)
|
|
|
|
|
#define NMEA_MAX_STATEMENT_ITEM_LENGTH (16)
|
|
|
|
|
#define NMEA_EVENT_LOOP_QUEUE_SIZE (16)
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* @brief Define of NMEA Parser Event base
|
|
|
|
|
*
|
|
|
|
|
*/
|
2018-11-19 03:54:57 -05:00
|
|
|
ESP_EVENT_DEFINE_BASE(ESP_NMEA_EVENT);
|
2018-11-28 05:49:18 -05:00
|
|
|
|
|
|
|
|
static const char *GPS_TAG = "nmea_parser";
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* @brief GPS parser library runtime structure
|
|
|
|
|
*
|
|
|
|
|
*/
|
|
|
|
|
typedef struct {
|
|
|
|
|
uint8_t item_pos; /*!< Current position in item */
|
|
|
|
|
uint8_t item_num; /*!< Current item number */
|
|
|
|
|
uint8_t asterisk; /*!< Asterisk detected flag */
|
|
|
|
|
uint8_t crc; /*!< Calculated CRC value */
|
|
|
|
|
uint8_t parsed_statement; /*!< OR'd of statements that have been parsed */
|
|
|
|
|
uint8_t sat_num; /*!< Satellite number */
|
|
|
|
|
uint8_t sat_count; /*!< Satellite count */
|
|
|
|
|
uint8_t cur_statement; /*!< Current statement ID */
|
|
|
|
|
uint32_t all_statements; /*!< All statements mask */
|
|
|
|
|
char item_str[NMEA_MAX_STATEMENT_ITEM_LENGTH]; /*!< Current item */
|
|
|
|
|
gps_t parent; /*!< Parent class */
|
|
|
|
|
uart_port_t uart_port; /*!< Uart port number */
|
|
|
|
|
uint8_t *buffer; /*!< Runtime buffer */
|
|
|
|
|
esp_event_loop_handle_t event_loop_hdl; /*!< Event loop handle */
|
|
|
|
|
TaskHandle_t tsk_hdl; /*!< NMEA Parser task handle */
|
|
|
|
|
QueueHandle_t event_queue; /*!< UART event queue handle */
|
|
|
|
|
} esp_gps_t;
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* @brief parse latitude or longitude
|
|
|
|
|
* format of latitude in NMEA is ddmm.sss and longitude is dddmm.sss
|
|
|
|
|
* @param esp_gps esp_gps_t type object
|
|
|
|
|
* @return float Latitude or Longitude value (unit: degree)
|
|
|
|
|
*/
|
|
|
|
|
static float parse_lat_long(esp_gps_t *esp_gps)
|
|
|
|
|
{
|
|
|
|
|
float ll = strtof(esp_gps->item_str, NULL);
|
|
|
|
|
int deg = ((int)ll) / 100;
|
|
|
|
|
float min = ll - (deg * 100);
|
|
|
|
|
ll = deg + min / 60.0f;
|
|
|
|
|
return ll;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* @brief Converter two continuous numeric character into a uint8_t number
|
|
|
|
|
*
|
|
|
|
|
* @param digit_char numeric character
|
|
|
|
|
* @return uint8_t result of converting
|
|
|
|
|
*/
|
|
|
|
|
static inline uint8_t convert_two_digit2number(const char *digit_char)
|
|
|
|
|
{
|
|
|
|
|
return 10 * (digit_char[0] - '0') + (digit_char[1] - '0');
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* @brief Parse UTC time in GPS statements
|
|
|
|
|
*
|
|
|
|
|
* @param esp_gps esp_gps_t type object
|
|
|
|
|
*/
|
|
|
|
|
static void parse_utc_time(esp_gps_t *esp_gps)
|
|
|
|
|
{
|
|
|
|
|
esp_gps->parent.tim.hour = convert_two_digit2number(esp_gps->item_str + 0);
|
|
|
|
|
esp_gps->parent.tim.minute = convert_two_digit2number(esp_gps->item_str + 2);
|
|
|
|
|
esp_gps->parent.tim.second = convert_two_digit2number(esp_gps->item_str + 4);
|
|
|
|
|
if (esp_gps->item_str[6] == '.') {
|
|
|
|
|
uint16_t tmp = 0;
|
|
|
|
|
uint8_t i = 7;
|
|
|
|
|
while (esp_gps->item_str[i]) {
|
|
|
|
|
tmp = 10 * tmp + esp_gps->item_str[i] - '0';
|
|
|
|
|
i++;
|
|
|
|
|
}
|
|
|
|
|
esp_gps->parent.tim.thousand = tmp;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_GGA
|
|
|
|
|
/**
|
|
|
|
|
* @brief Parse GGA statements
|
|
|
|
|
*
|
|
|
|
|
* @param esp_gps esp_gps_t type object
|
|
|
|
|
*/
|
|
|
|
|
static void parse_gga(esp_gps_t *esp_gps)
|
|
|
|
|
{
|
|
|
|
|
/* Process GGA statement */
|
|
|
|
|
switch (esp_gps->item_num) {
|
|
|
|
|
case 1: /* Process UTC time */
|
|
|
|
|
parse_utc_time(esp_gps);
|
|
|
|
|
break;
|
|
|
|
|
case 2: /* Latitude */
|
|
|
|
|
esp_gps->parent.latitude = parse_lat_long(esp_gps);
|
|
|
|
|
break;
|
|
|
|
|
case 3: /* Latitude north(1)/south(-1) information */
|
|
|
|
|
if (esp_gps->item_str[0] == 'S' || esp_gps->item_str[0] == 's') {
|
|
|
|
|
esp_gps->parent.latitude *= -1;
|
|
|
|
|
}
|
|
|
|
|
break;
|
|
|
|
|
case 4: /* Longitude */
|
|
|
|
|
esp_gps->parent.longitude = parse_lat_long(esp_gps);
|
|
|
|
|
break;
|
|
|
|
|
case 5: /* Longitude east(1)/west(-1) information */
|
|
|
|
|
if (esp_gps->item_str[0] == 'W' || esp_gps->item_str[0] == 'w') {
|
|
|
|
|
esp_gps->parent.longitude *= -1;
|
|
|
|
|
}
|
|
|
|
|
break;
|
|
|
|
|
case 6: /* Fix status */
|
|
|
|
|
esp_gps->parent.fix = (gps_fix_t)strtol(esp_gps->item_str, NULL, 10);
|
|
|
|
|
break;
|
|
|
|
|
case 7: /* Satellites in use */
|
|
|
|
|
esp_gps->parent.sats_in_use = (uint8_t)strtol(esp_gps->item_str, NULL, 10);
|
|
|
|
|
break;
|
|
|
|
|
case 8: /* HDOP */
|
|
|
|
|
esp_gps->parent.dop_h = strtof(esp_gps->item_str, NULL);
|
|
|
|
|
break;
|
|
|
|
|
case 9: /* Altitude */
|
|
|
|
|
esp_gps->parent.altitude = strtof(esp_gps->item_str, NULL);
|
|
|
|
|
break;
|
|
|
|
|
case 11: /* Altitude above ellipsoid */
|
|
|
|
|
esp_gps->parent.altitude += strtof(esp_gps->item_str, NULL);
|
|
|
|
|
break;
|
|
|
|
|
default:
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_GSA
|
|
|
|
|
/**
|
|
|
|
|
* @brief Parse GSA statements
|
|
|
|
|
*
|
|
|
|
|
* @param esp_gps esp_gps_t type object
|
|
|
|
|
*/
|
|
|
|
|
static void parse_gsa(esp_gps_t *esp_gps)
|
|
|
|
|
{
|
|
|
|
|
/* Process GSA statement */
|
|
|
|
|
switch (esp_gps->item_num) {
|
|
|
|
|
case 2: /* Process fix mode */
|
|
|
|
|
esp_gps->parent.fix_mode = (gps_fix_mode_t)strtol(esp_gps->item_str, NULL, 10);
|
|
|
|
|
break;
|
|
|
|
|
case 15: /* Process PDOP */
|
|
|
|
|
esp_gps->parent.dop_p = strtof(esp_gps->item_str, NULL);
|
|
|
|
|
break;
|
|
|
|
|
case 16: /* Process HDOP */
|
|
|
|
|
esp_gps->parent.dop_h = strtof(esp_gps->item_str, NULL);
|
|
|
|
|
break;
|
|
|
|
|
case 17: /* Process VDOP */
|
|
|
|
|
esp_gps->parent.dop_v = strtof(esp_gps->item_str, NULL);
|
|
|
|
|
break;
|
|
|
|
|
default:
|
|
|
|
|
/* Parse satellite IDs */
|
|
|
|
|
if (esp_gps->item_num >= 3 && esp_gps->item_num <= 14) {
|
|
|
|
|
esp_gps->parent.sats_id_in_use[esp_gps->item_num - 3] = (uint8_t)strtol(esp_gps->item_str, NULL, 10);
|
|
|
|
|
}
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_GSV
|
|
|
|
|
/**
|
|
|
|
|
* @brief Parse GSV statements
|
|
|
|
|
*
|
|
|
|
|
* @param esp_gps esp_gps_t type object
|
|
|
|
|
*/
|
|
|
|
|
static void parse_gsv(esp_gps_t *esp_gps)
|
|
|
|
|
{
|
|
|
|
|
/* Process GSV statement */
|
|
|
|
|
switch (esp_gps->item_num) {
|
|
|
|
|
case 1: /* total GSV numbers */
|
|
|
|
|
esp_gps->sat_count = (uint8_t)strtol(esp_gps->item_str, NULL, 10);
|
2018-12-10 03:17:51 -05:00
|
|
|
break;
|
2018-11-28 05:49:18 -05:00
|
|
|
case 2: /* Current GSV statement number */
|
|
|
|
|
esp_gps->sat_num = (uint8_t)strtol(esp_gps->item_str, NULL, 10);
|
|
|
|
|
break;
|
|
|
|
|
case 3: /* Process satellites in view */
|
|
|
|
|
esp_gps->parent.sats_in_view = (uint8_t)strtol(esp_gps->item_str, NULL, 10);
|
|
|
|
|
break;
|
|
|
|
|
default:
|
|
|
|
|
if (esp_gps->item_num >= 4 && esp_gps->item_num <= 19) {
|
|
|
|
|
uint8_t item_num = esp_gps->item_num - 4; /* Normalize item number from 4-19 to 0-15 */
|
|
|
|
|
uint8_t index;
|
|
|
|
|
uint32_t value;
|
|
|
|
|
index = 4 * (esp_gps->sat_num - 1) + item_num / 4; /* Get array index */
|
|
|
|
|
if (index < GPS_MAX_SATELLITES_IN_VIEW) {
|
|
|
|
|
value = strtol(esp_gps->item_str, NULL, 10);
|
|
|
|
|
switch (item_num % 4) {
|
|
|
|
|
case 0:
|
|
|
|
|
esp_gps->parent.sats_desc_in_view[index].num = (uint8_t)value;
|
|
|
|
|
break;
|
|
|
|
|
case 1:
|
|
|
|
|
esp_gps->parent.sats_desc_in_view[index].elevation = (uint8_t)value;
|
|
|
|
|
break;
|
|
|
|
|
case 2:
|
|
|
|
|
esp_gps->parent.sats_desc_in_view[index].azimuth = (uint16_t)value;
|
|
|
|
|
break;
|
|
|
|
|
case 3:
|
|
|
|
|
esp_gps->parent.sats_desc_in_view[index].snr = (uint8_t)value;
|
|
|
|
|
break;
|
|
|
|
|
default:
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_RMC
|
|
|
|
|
/**
|
|
|
|
|
* @brief Parse RMC statements
|
|
|
|
|
*
|
|
|
|
|
* @param esp_gps esp_gps_t type object
|
|
|
|
|
*/
|
|
|
|
|
static void parse_rmc(esp_gps_t *esp_gps)
|
|
|
|
|
{
|
|
|
|
|
/* Process GPRMC statement */
|
|
|
|
|
switch (esp_gps->item_num) {
|
|
|
|
|
case 1:/* Process UTC time */
|
|
|
|
|
parse_utc_time(esp_gps);
|
|
|
|
|
break;
|
|
|
|
|
case 2: /* Process valid status */
|
|
|
|
|
esp_gps->parent.valid = (esp_gps->item_str[0] == 'A');
|
|
|
|
|
break;
|
|
|
|
|
case 3:/* Latitude */
|
|
|
|
|
esp_gps->parent.latitude = parse_lat_long(esp_gps);
|
|
|
|
|
break;
|
|
|
|
|
case 4: /* Latitude north(1)/south(-1) information */
|
|
|
|
|
if (esp_gps->item_str[0] == 'S' || esp_gps->item_str[0] == 's') {
|
|
|
|
|
esp_gps->parent.latitude *= -1;
|
|
|
|
|
}
|
|
|
|
|
break;
|
|
|
|
|
case 5: /* Longitude */
|
|
|
|
|
esp_gps->parent.longitude = parse_lat_long(esp_gps);
|
|
|
|
|
break;
|
|
|
|
|
case 6: /* Longitude east(1)/west(-1) information */
|
|
|
|
|
if (esp_gps->item_str[0] == 'W' || esp_gps->item_str[0] == 'w') {
|
|
|
|
|
esp_gps->parent.longitude *= -1;
|
|
|
|
|
}
|
|
|
|
|
break;
|
|
|
|
|
case 7: /* Process ground speed in unit m/s */
|
|
|
|
|
esp_gps->parent.speed = strtof(esp_gps->item_str, NULL) * 1.852;
|
|
|
|
|
break;
|
|
|
|
|
case 8: /* Process true course over ground */
|
|
|
|
|
esp_gps->parent.cog = strtof(esp_gps->item_str, NULL);
|
|
|
|
|
break;
|
|
|
|
|
case 9: /* Process date */
|
|
|
|
|
esp_gps->parent.date.day = convert_two_digit2number(esp_gps->item_str + 0);
|
|
|
|
|
esp_gps->parent.date.month = convert_two_digit2number(esp_gps->item_str + 2);
|
|
|
|
|
esp_gps->parent.date.year = convert_two_digit2number(esp_gps->item_str + 4);
|
|
|
|
|
break;
|
|
|
|
|
case 10: /* Process magnetic variation */
|
|
|
|
|
esp_gps->parent.variation = strtof(esp_gps->item_str, NULL);
|
|
|
|
|
break;
|
|
|
|
|
default:
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_GLL
|
|
|
|
|
/**
|
|
|
|
|
* @brief Parse GLL statements
|
|
|
|
|
*
|
|
|
|
|
* @param esp_gps esp_gps_t type object
|
|
|
|
|
*/
|
|
|
|
|
static void parse_gll(esp_gps_t *esp_gps)
|
|
|
|
|
{
|
|
|
|
|
/* Process GPGLL statement */
|
|
|
|
|
switch (esp_gps->item_num) {
|
|
|
|
|
case 1:/* Latitude */
|
|
|
|
|
esp_gps->parent.latitude = parse_lat_long(esp_gps);
|
|
|
|
|
break;
|
|
|
|
|
case 2: /* Latitude north(1)/south(-1) information */
|
|
|
|
|
if (esp_gps->item_str[0] == 'S' || esp_gps->item_str[0] == 's') {
|
|
|
|
|
esp_gps->parent.latitude *= -1;
|
|
|
|
|
}
|
|
|
|
|
break;
|
|
|
|
|
case 3: /* Longitude */
|
|
|
|
|
esp_gps->parent.longitude = parse_lat_long(esp_gps);
|
|
|
|
|
break;
|
|
|
|
|
case 4: /* Longitude east(1)/west(-1) information */
|
|
|
|
|
if (esp_gps->item_str[0] == 'W' || esp_gps->item_str[0] == 'w') {
|
|
|
|
|
esp_gps->parent.longitude *= -1;
|
|
|
|
|
}
|
|
|
|
|
break;
|
|
|
|
|
case 5:/* Process UTC time */
|
|
|
|
|
parse_utc_time(esp_gps);
|
|
|
|
|
break;
|
|
|
|
|
case 6: /* Process valid status */
|
|
|
|
|
esp_gps->parent.valid = (esp_gps->item_str[0] == 'A');
|
|
|
|
|
break;
|
|
|
|
|
default:
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_VTG
|
|
|
|
|
/**
|
|
|
|
|
* @brief Parse VTG statements
|
|
|
|
|
*
|
|
|
|
|
* @param esp_gps esp_gps_t type object
|
|
|
|
|
*/
|
|
|
|
|
static void parse_vtg(esp_gps_t *esp_gps)
|
|
|
|
|
{
|
|
|
|
|
/* Process GPVGT statement */
|
|
|
|
|
switch (esp_gps->item_num) {
|
|
|
|
|
case 1: /* Process true course over ground */
|
|
|
|
|
esp_gps->parent.cog = strtof(esp_gps->item_str, NULL);
|
|
|
|
|
break;
|
|
|
|
|
case 3:/* Process magnetic variation */
|
|
|
|
|
esp_gps->parent.variation = strtof(esp_gps->item_str, NULL);
|
|
|
|
|
break;
|
|
|
|
|
case 5:/* Process ground speed in unit m/s */
|
|
|
|
|
esp_gps->parent.speed = strtof(esp_gps->item_str, NULL) * 1.852;//knots to m/s
|
|
|
|
|
break;
|
|
|
|
|
case 7:/* Process ground speed in unit m/s */
|
|
|
|
|
esp_gps->parent.speed = strtof(esp_gps->item_str, NULL) / 3.6;//km/h to m/s
|
|
|
|
|
break;
|
|
|
|
|
default:
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* @brief Parse received item
|
|
|
|
|
*
|
|
|
|
|
* @param esp_gps esp_gps_t type object
|
|
|
|
|
* @return esp_err_t ESP_OK on success, ESP_FAIL on error
|
|
|
|
|
*/
|
|
|
|
|
static esp_err_t parse_item(esp_gps_t *esp_gps)
|
|
|
|
|
{
|
|
|
|
|
esp_err_t err = ESP_OK;
|
|
|
|
|
/* start of a statement */
|
|
|
|
|
if (esp_gps->item_num == 0 && esp_gps->item_str[0] == '$') {
|
|
|
|
|
if (0) {
|
|
|
|
|
}
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_GGA
|
|
|
|
|
else if (strstr(esp_gps->item_str, "GGA")) {
|
|
|
|
|
esp_gps->cur_statement = STATEMENT_GGA;
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_GSA
|
|
|
|
|
else if (strstr(esp_gps->item_str, "GSA")) {
|
|
|
|
|
esp_gps->cur_statement = STATEMENT_GSA;
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_RMC
|
|
|
|
|
else if (strstr(esp_gps->item_str, "RMC")) {
|
|
|
|
|
esp_gps->cur_statement = STATEMENT_RMC;
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_GSV
|
|
|
|
|
else if (strstr(esp_gps->item_str, "GSV")) {
|
|
|
|
|
esp_gps->cur_statement = STATEMENT_GSV;
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_GLL
|
|
|
|
|
else if (strstr(esp_gps->item_str, "GLL")) {
|
|
|
|
|
esp_gps->cur_statement = STATEMENT_GLL;
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_VTG
|
|
|
|
|
else if (strstr(esp_gps->item_str, "VTG")) {
|
|
|
|
|
esp_gps->cur_statement = STATEMENT_VTG;
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
else {
|
|
|
|
|
esp_gps->cur_statement = STATEMENT_UNKNOWN;
|
|
|
|
|
}
|
|
|
|
|
goto out;
|
|
|
|
|
}
|
|
|
|
|
/* Parse each item, depend on the type of the statement */
|
|
|
|
|
if (esp_gps->cur_statement == STATEMENT_UNKNOWN) {
|
|
|
|
|
goto out;
|
|
|
|
|
}
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_GGA
|
|
|
|
|
else if (esp_gps->cur_statement == STATEMENT_GGA) {
|
|
|
|
|
parse_gga(esp_gps);
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_GSA
|
|
|
|
|
else if (esp_gps->cur_statement == STATEMENT_GSA) {
|
|
|
|
|
parse_gsa(esp_gps);
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_GSV
|
|
|
|
|
else if (esp_gps->cur_statement == STATEMENT_GSV) {
|
|
|
|
|
parse_gsv(esp_gps);
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_RMC
|
|
|
|
|
else if (esp_gps->cur_statement == STATEMENT_RMC) {
|
|
|
|
|
parse_rmc(esp_gps);
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_GLL
|
|
|
|
|
else if (esp_gps->cur_statement == STATEMENT_GLL) {
|
|
|
|
|
parse_gll(esp_gps);
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_VTG
|
|
|
|
|
else if (esp_gps->cur_statement == STATEMENT_VTG) {
|
|
|
|
|
parse_vtg(esp_gps);
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
else {
|
|
|
|
|
err = ESP_FAIL;
|
|
|
|
|
}
|
|
|
|
|
out:
|
|
|
|
|
return err;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* @brief Parse NMEA statements from GPS receiver
|
|
|
|
|
*
|
|
|
|
|
* @param esp_gps esp_gps_t type object
|
|
|
|
|
* @param len number of bytes to decode
|
|
|
|
|
* @return esp_err_t ESP_OK on success, ESP_FAIL on error
|
|
|
|
|
*/
|
|
|
|
|
static esp_err_t gps_decode(esp_gps_t *esp_gps, size_t len)
|
|
|
|
|
{
|
|
|
|
|
const uint8_t *d = esp_gps->buffer;
|
|
|
|
|
while (*d) {
|
|
|
|
|
/* Start of a statement */
|
|
|
|
|
if (*d == '$') {
|
|
|
|
|
/* Reset runtime information */
|
|
|
|
|
esp_gps->asterisk = 0;
|
|
|
|
|
esp_gps->item_num = 0;
|
|
|
|
|
esp_gps->item_pos = 0;
|
|
|
|
|
esp_gps->cur_statement = 0;
|
|
|
|
|
esp_gps->crc = 0;
|
|
|
|
|
esp_gps->sat_count = 0;
|
|
|
|
|
esp_gps->sat_num = 0;
|
|
|
|
|
/* Add character to item */
|
|
|
|
|
esp_gps->item_str[esp_gps->item_pos++] = *d;
|
|
|
|
|
esp_gps->item_str[esp_gps->item_pos] = '\0';
|
|
|
|
|
}
|
|
|
|
|
/* Detect item separator character */
|
|
|
|
|
else if (*d == ',') {
|
|
|
|
|
/* Parse current item */
|
|
|
|
|
parse_item(esp_gps);
|
|
|
|
|
/* Add character to CRC computation */
|
|
|
|
|
esp_gps->crc ^= (uint8_t)(*d);
|
|
|
|
|
/* Start with next item */
|
|
|
|
|
esp_gps->item_pos = 0;
|
|
|
|
|
esp_gps->item_str[0] = '\0';
|
|
|
|
|
esp_gps->item_num++;
|
|
|
|
|
}
|
|
|
|
|
/* End of CRC computation */
|
|
|
|
|
else if (*d == '*') {
|
|
|
|
|
/* Parse current item */
|
|
|
|
|
parse_item(esp_gps);
|
|
|
|
|
/* Asterisk detected */
|
|
|
|
|
esp_gps->asterisk = 1;
|
|
|
|
|
/* Start with next item */
|
|
|
|
|
esp_gps->item_pos = 0;
|
|
|
|
|
esp_gps->item_str[0] = '\0';
|
|
|
|
|
esp_gps->item_num++;
|
|
|
|
|
}
|
|
|
|
|
/* End of statement */
|
|
|
|
|
else if (*d == '\r') {
|
|
|
|
|
/* Convert received CRC from string (hex) to number */
|
|
|
|
|
uint8_t crc = (uint8_t)strtol(esp_gps->item_str, NULL, 16);
|
|
|
|
|
/* CRC passed */
|
|
|
|
|
if (esp_gps->crc == crc) {
|
|
|
|
|
switch (esp_gps->cur_statement) {
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_GGA
|
|
|
|
|
case STATEMENT_GGA:
|
|
|
|
|
esp_gps->parsed_statement |= 1 << STATEMENT_GGA;
|
|
|
|
|
break;
|
|
|
|
|
#endif
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_GSA
|
|
|
|
|
case STATEMENT_GSA:
|
|
|
|
|
esp_gps->parsed_statement |= 1 << STATEMENT_GSA;
|
|
|
|
|
break;
|
|
|
|
|
#endif
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_RMC
|
|
|
|
|
case STATEMENT_RMC:
|
|
|
|
|
esp_gps->parsed_statement |= 1 << STATEMENT_RMC;
|
|
|
|
|
break;
|
|
|
|
|
#endif
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_GSV
|
|
|
|
|
case STATEMENT_GSV:
|
|
|
|
|
if (esp_gps->sat_num == esp_gps->sat_count) {
|
|
|
|
|
esp_gps->parsed_statement |= 1 << STATEMENT_GSV;
|
|
|
|
|
}
|
|
|
|
|
break;
|
|
|
|
|
#endif
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_GLL
|
|
|
|
|
case STATEMENT_GLL:
|
|
|
|
|
esp_gps->parsed_statement |= 1 << STATEMENT_GLL;
|
|
|
|
|
break;
|
|
|
|
|
#endif
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_VTG
|
|
|
|
|
case STATEMENT_VTG:
|
|
|
|
|
esp_gps->parsed_statement |= 1 << STATEMENT_VTG;
|
|
|
|
|
break;
|
|
|
|
|
#endif
|
|
|
|
|
default:
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
/* Check if all statements have been parsed */
|
|
|
|
|
if (((esp_gps->parsed_statement) & esp_gps->all_statements) == esp_gps->all_statements) {
|
|
|
|
|
esp_gps->parsed_statement = 0;
|
|
|
|
|
/* Send signal to notify that GPS information has been updated */
|
|
|
|
|
esp_event_post_to(esp_gps->event_loop_hdl, ESP_NMEA_EVENT, GPS_UPDATE,
|
|
|
|
|
&(esp_gps->parent), sizeof(gps_t), 100 / portTICK_PERIOD_MS);
|
|
|
|
|
}
|
|
|
|
|
} else {
|
|
|
|
|
ESP_LOGD(GPS_TAG, "CRC Error for statement:%s", esp_gps->buffer);
|
|
|
|
|
}
|
|
|
|
|
if (esp_gps->cur_statement == STATEMENT_UNKNOWN) {
|
|
|
|
|
/* Send signal to notify that one unknown statement has been met */
|
|
|
|
|
esp_event_post_to(esp_gps->event_loop_hdl, ESP_NMEA_EVENT, GPS_UNKNOWN,
|
|
|
|
|
esp_gps->buffer, len, 100 / portTICK_PERIOD_MS);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
/* Other non-space character */
|
|
|
|
|
else {
|
|
|
|
|
if (!(esp_gps->asterisk)) {
|
|
|
|
|
/* Add to CRC */
|
|
|
|
|
esp_gps->crc ^= (uint8_t)(*d);
|
|
|
|
|
}
|
|
|
|
|
/* Add character to item */
|
|
|
|
|
esp_gps->item_str[esp_gps->item_pos++] = *d;
|
|
|
|
|
esp_gps->item_str[esp_gps->item_pos] = '\0';
|
|
|
|
|
}
|
|
|
|
|
/* Process next character */
|
|
|
|
|
d++;
|
|
|
|
|
}
|
|
|
|
|
return ESP_OK;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* @brief Handle when a pattern has been detected by uart
|
|
|
|
|
*
|
|
|
|
|
* @param esp_gps esp_gps_t type object
|
|
|
|
|
*/
|
|
|
|
|
static void esp_handle_uart_pattern(esp_gps_t *esp_gps)
|
|
|
|
|
{
|
|
|
|
|
int pos = uart_pattern_pop_pos(esp_gps->uart_port);
|
|
|
|
|
if (pos != -1) {
|
|
|
|
|
/* read one line(include '\n') */
|
|
|
|
|
int read_len = uart_read_bytes(esp_gps->uart_port, esp_gps->buffer, pos + 1, 100 / portTICK_PERIOD_MS);
|
|
|
|
|
/* make sure the line is a standard string */
|
|
|
|
|
esp_gps->buffer[read_len] = '\0';
|
|
|
|
|
/* Send new line to handle */
|
|
|
|
|
if (gps_decode(esp_gps, read_len + 1) != ESP_OK) {
|
|
|
|
|
ESP_LOGW(GPS_TAG, "GPS decode line failed");
|
|
|
|
|
}
|
|
|
|
|
} else {
|
|
|
|
|
ESP_LOGW(GPS_TAG, "Pattern Queue Size too small");
|
|
|
|
|
uart_flush_input(esp_gps->uart_port);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* @brief NMEA Parser Task Entry
|
|
|
|
|
*
|
|
|
|
|
* @param arg argument
|
|
|
|
|
*/
|
|
|
|
|
static void nmea_parser_task_entry(void *arg)
|
|
|
|
|
{
|
|
|
|
|
esp_gps_t *esp_gps = (esp_gps_t *)arg;
|
|
|
|
|
uart_event_t event;
|
|
|
|
|
while (1) {
|
|
|
|
|
if (xQueueReceive(esp_gps->event_queue, &event, pdMS_TO_TICKS(200))) {
|
|
|
|
|
switch (event.type) {
|
|
|
|
|
case UART_DATA:
|
|
|
|
|
break;
|
|
|
|
|
case UART_FIFO_OVF:
|
|
|
|
|
ESP_LOGW(GPS_TAG, "HW FIFO Overflow");
|
|
|
|
|
uart_flush(esp_gps->uart_port);
|
|
|
|
|
xQueueReset(esp_gps->event_queue);
|
|
|
|
|
break;
|
|
|
|
|
case UART_BUFFER_FULL:
|
|
|
|
|
ESP_LOGW(GPS_TAG, "Ring Buffer Full");
|
|
|
|
|
uart_flush(esp_gps->uart_port);
|
|
|
|
|
xQueueReset(esp_gps->event_queue);
|
|
|
|
|
break;
|
|
|
|
|
case UART_BREAK:
|
|
|
|
|
ESP_LOGW(GPS_TAG, "Rx Break");
|
|
|
|
|
break;
|
|
|
|
|
case UART_PARITY_ERR:
|
|
|
|
|
ESP_LOGE(GPS_TAG, "Parity Error");
|
|
|
|
|
break;
|
|
|
|
|
case UART_FRAME_ERR:
|
|
|
|
|
ESP_LOGE(GPS_TAG, "Frame Error");
|
|
|
|
|
break;
|
|
|
|
|
case UART_PATTERN_DET:
|
|
|
|
|
esp_handle_uart_pattern(esp_gps);
|
|
|
|
|
break;
|
|
|
|
|
default:
|
|
|
|
|
ESP_LOGW(GPS_TAG, "unknown uart event type: %d", event.type);
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
/* Drive the event loop */
|
|
|
|
|
esp_event_loop_run(esp_gps->event_loop_hdl, pdMS_TO_TICKS(50));
|
|
|
|
|
}
|
|
|
|
|
vTaskDelete(NULL);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* @brief Init NMEA Parser
|
|
|
|
|
*
|
|
|
|
|
* @param config Configuration of NMEA Parser
|
|
|
|
|
* @return nmea_parser_handle_t handle of nmea_parser
|
|
|
|
|
*/
|
|
|
|
|
nmea_parser_handle_t nmea_parser_init(const nmea_parser_config_t *config)
|
|
|
|
|
{
|
|
|
|
|
esp_gps_t *esp_gps = calloc(1, sizeof(esp_gps_t));
|
|
|
|
|
if (!esp_gps) {
|
|
|
|
|
ESP_LOGE(GPS_TAG, "calloc memory for esp_fps failed");
|
|
|
|
|
goto err_gps;
|
|
|
|
|
}
|
|
|
|
|
esp_gps->buffer = calloc(1, NMEA_PARSER_RUNTIME_BUFFER_SIZE);
|
|
|
|
|
if (!esp_gps->buffer) {
|
|
|
|
|
ESP_LOGE(GPS_TAG, "calloc memory for runtime buffer failed");
|
|
|
|
|
goto err_buffer;
|
|
|
|
|
}
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_GSA
|
|
|
|
|
esp_gps->all_statements |= (1 << STATEMENT_GSA);
|
|
|
|
|
#endif
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_GSV
|
|
|
|
|
esp_gps->all_statements |= (1 << STATEMENT_GSV);
|
|
|
|
|
#endif
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_GGA
|
|
|
|
|
esp_gps->all_statements |= (1 << STATEMENT_GGA);
|
|
|
|
|
#endif
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_RMC
|
|
|
|
|
esp_gps->all_statements |= (1 << STATEMENT_RMC);
|
|
|
|
|
#endif
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_GLL
|
|
|
|
|
esp_gps->all_statements |= (1 << STATEMENT_GLL);
|
|
|
|
|
#endif
|
|
|
|
|
#if CONFIG_NMEA_STATEMENT_VTG
|
|
|
|
|
esp_gps->all_statements |= (1 << STATEMENT_VTG);
|
|
|
|
|
#endif
|
|
|
|
|
/* Set attributes */
|
|
|
|
|
esp_gps->uart_port = config->uart.uart_port;
|
|
|
|
|
esp_gps->all_statements &= 0xFE;
|
|
|
|
|
/* Install UART friver */
|
|
|
|
|
uart_config_t uart_config = {
|
|
|
|
|
.baud_rate = config->uart.baud_rate,
|
|
|
|
|
.data_bits = config->uart.data_bits,
|
|
|
|
|
.parity = config->uart.parity,
|
|
|
|
|
.stop_bits = config->uart.stop_bits,
|
|
|
|
|
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE
|
|
|
|
|
};
|
|
|
|
|
if (uart_param_config(esp_gps->uart_port, &uart_config) != ESP_OK) {
|
|
|
|
|
ESP_LOGE(GPS_TAG, "config uart parameter failed");
|
|
|
|
|
goto err_uart_config;
|
|
|
|
|
}
|
|
|
|
|
if (uart_set_pin(esp_gps->uart_port, UART_PIN_NO_CHANGE, config->uart.rx_pin,
|
|
|
|
|
UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE) != ESP_OK) {
|
|
|
|
|
ESP_LOGE(GPS_TAG, "config uart gpio failed");
|
|
|
|
|
goto err_uart_config;
|
|
|
|
|
}
|
|
|
|
|
if (uart_driver_install(esp_gps->uart_port, CONFIG_NMEA_PARSER_RING_BUFFER_SIZE, 0,
|
|
|
|
|
config->uart.event_queue_size, &esp_gps->event_queue, 0) != ESP_OK) {
|
|
|
|
|
ESP_LOGE(GPS_TAG, "install uart driver failed");
|
|
|
|
|
goto err_uart_install;
|
|
|
|
|
}
|
|
|
|
|
/* Set pattern interrupt, used to detect the end of a line */
|
|
|
|
|
uart_enable_pattern_det_intr(esp_gps->uart_port, '\n', 1, 10000, 10, 10);
|
|
|
|
|
/* Set pattern queue size */
|
|
|
|
|
uart_pattern_queue_reset(esp_gps->uart_port, config->uart.event_queue_size);
|
|
|
|
|
uart_flush(esp_gps->uart_port);
|
|
|
|
|
/* Create Event loop */
|
|
|
|
|
esp_event_loop_args_t loop_args = {
|
|
|
|
|
.queue_size = NMEA_EVENT_LOOP_QUEUE_SIZE,
|
|
|
|
|
.task_name = NULL
|
|
|
|
|
};
|
|
|
|
|
if (esp_event_loop_create(&loop_args, &esp_gps->event_loop_hdl) != ESP_OK) {
|
|
|
|
|
ESP_LOGE(GPS_TAG, "create event loop faild");
|
|
|
|
|
goto err_eloop;
|
|
|
|
|
}
|
|
|
|
|
/* Create NMEA Parser task */
|
|
|
|
|
BaseType_t err = xTaskCreate(
|
|
|
|
|
nmea_parser_task_entry,
|
|
|
|
|
"nmea_parser",
|
|
|
|
|
CONFIG_NMEA_PARSER_TASK_STACK_SIZE,
|
|
|
|
|
esp_gps,
|
|
|
|
|
CONFIG_NMEA_PARSER_TASK_PRIORITY,
|
|
|
|
|
&esp_gps->tsk_hdl);
|
|
|
|
|
if (err != pdTRUE) {
|
|
|
|
|
ESP_LOGE(GPS_TAG, "create NMEA Parser task failed");
|
|
|
|
|
goto err_task_create;
|
|
|
|
|
}
|
|
|
|
|
ESP_LOGI(GPS_TAG, "NMEA Parser init OK");
|
|
|
|
|
return esp_gps;
|
|
|
|
|
/*Error Handling*/
|
|
|
|
|
err_task_create:
|
|
|
|
|
esp_event_loop_delete(esp_gps->event_loop_hdl);
|
|
|
|
|
err_eloop:
|
|
|
|
|
err_uart_install:
|
|
|
|
|
uart_driver_delete(esp_gps->uart_port);
|
|
|
|
|
err_uart_config:
|
|
|
|
|
err_buffer:
|
|
|
|
|
free(esp_gps->buffer);
|
|
|
|
|
err_gps:
|
|
|
|
|
free(esp_gps);
|
|
|
|
|
return NULL;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* @brief Deinit NMEA Parser
|
|
|
|
|
*
|
|
|
|
|
* @param nmea_hdl handle of NMEA parser
|
|
|
|
|
* @return esp_err_t ESP_OK on success,ESP_FAIL on error
|
|
|
|
|
*/
|
|
|
|
|
esp_err_t nmea_parser_deinit(nmea_parser_handle_t nmea_hdl)
|
|
|
|
|
{
|
|
|
|
|
esp_gps_t *esp_gps = (esp_gps_t *)nmea_hdl;
|
|
|
|
|
vTaskDelete(esp_gps->tsk_hdl);
|
|
|
|
|
esp_event_loop_delete(esp_gps->event_loop_hdl);
|
|
|
|
|
esp_err_t err = uart_driver_delete(esp_gps->uart_port);
|
|
|
|
|
free(esp_gps->buffer);
|
|
|
|
|
free(esp_gps);
|
|
|
|
|
return err;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* @brief Add user defined handler for NMEA parser
|
|
|
|
|
*
|
|
|
|
|
* @param nmea_hdl handle of NMEA parser
|
|
|
|
|
* @param event_handler user defined event handler
|
|
|
|
|
* @param handler_args handler specific arguments
|
|
|
|
|
* @return esp_err_t
|
|
|
|
|
* - ESP_OK: Success
|
|
|
|
|
* - ESP_ERR_NO_MEM: Cannot allocate memory for the handler
|
|
|
|
|
* - ESP_ERR_INVALIG_ARG: Invalid combination of event base and event id
|
|
|
|
|
* - Others: Fail
|
|
|
|
|
*/
|
|
|
|
|
esp_err_t nmea_parser_add_handler(nmea_parser_handle_t nmea_hdl, esp_event_handler_t event_handler, void *handler_args)
|
|
|
|
|
{
|
|
|
|
|
esp_gps_t *esp_gps = (esp_gps_t *)nmea_hdl;
|
|
|
|
|
return esp_event_handler_register_with(esp_gps->event_loop_hdl, ESP_NMEA_EVENT, ESP_EVENT_ANY_ID,
|
|
|
|
|
event_handler, handler_args);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* @brief Remove user defined handler for NMEA parser
|
|
|
|
|
*
|
|
|
|
|
* @param nmea_hdl handle of NMEA parser
|
|
|
|
|
* @param event_handler user defined event handler
|
|
|
|
|
* @return esp_err_t
|
|
|
|
|
* - ESP_OK: Success
|
|
|
|
|
* - ESP_ERR_INVALIG_ARG: Invalid combination of event base and event id
|
|
|
|
|
* - Others: Fail
|
|
|
|
|
*/
|
|
|
|
|
esp_err_t nmea_parser_remove_handler(nmea_parser_handle_t nmea_hdl, esp_event_handler_t event_handler)
|
|
|
|
|
{
|
|
|
|
|
esp_gps_t *esp_gps = (esp_gps_t *)nmea_hdl;
|
|
|
|
|
return esp_event_handler_unregister_with(esp_gps->event_loop_hdl, ESP_NMEA_EVENT, ESP_EVENT_ANY_ID, event_handler);
|
|
|
|
|
}
|
