diff --git a/platformio.ini b/platformio.ini index 69003c3..5e5ed36 100644 --- a/platformio.ini +++ b/platformio.ini @@ -23,6 +23,9 @@ build_flags = -D__gpsdebug__ ;Debug GPS coordinates if no active GPS signal found. -D__gps__ -DCORE_DEBUG_LEVEL=5 + -DCONFIG_NMEA_STATEMENT_GGA + -DCONFIG_NMEA_STATEMENT_RMC + -DCONFIG_NMEA_STATEMENT_GLL build_type = debug monitor_filters = esp32_exception_decoder lib_deps = diff --git a/src/calculate/nmea_parser.c b/src/calculate/nmea_parser.c new file mode 100644 index 0000000..5ff4645 --- /dev/null +++ b/src/calculate/nmea_parser.c @@ -0,0 +1,800 @@ +/* + * SPDX-FileCopyrightText: 2015-2022 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "esp_log.h" +#include "nmea_parser.h" + +/** + * @brief NMEA Parser runtime buffer size + * + */ +#define CONFIG_NMEA_PARSER_RING_BUFFER_SIZE (1024) +#define NMEA_PARSER_RUNTIME_BUFFER_SIZE (1024 / 2) +#define NMEA_MAX_STATEMENT_ITEM_LENGTH (16) +#define NMEA_EVENT_LOOP_QUEUE_SIZE (16) + +/** + * @brief Define of NMEA Parser Event base + * + */ +ESP_EVENT_DEFINE_BASE(ESP_NMEA_EVENT); + +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); + break; + 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, + .source_clk = UART_SCLK_APB, + }; + 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; + } + 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; + } + /* Set pattern interrupt, used to detect the end of a line */ + uart_enable_pattern_det_baud_intr(esp_gps->uart_port, '\n', 1, 9, 0, 0); + /* 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", + 4096, + esp_gps, + 1, + &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); +} + + +uart_config_t init_uart_config(){ +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, + .rx_flow_ctrl_thresh = 122, + .source_clk = UART_SCLK_APB,}; + return uart_config; +} \ No newline at end of file diff --git a/src/calculate/nmea_parser.h b/src/calculate/nmea_parser.h new file mode 100644 index 0000000..ddd51a1 --- /dev/null +++ b/src/calculate/nmea_parser.h @@ -0,0 +1,212 @@ +/* + * SPDX-FileCopyrightText: 2015-2022 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "esp_types.h" +#include "esp_event.h" +#include "esp_err.h" +#include "driver/uart.h" + +#define GPS_MAX_SATELLITES_IN_USE (12) +#define GPS_MAX_SATELLITES_IN_VIEW (16) + +/** + * @brief Declare of NMEA Parser Event base + * + */ +ESP_EVENT_DECLARE_BASE(ESP_NMEA_EVENT); + +/** + * @brief GPS fix type + * + */ +typedef enum { + GPS_FIX_INVALID, /*!< Not fixed */ + GPS_FIX_GPS, /*!< GPS */ + GPS_FIX_DGPS, /*!< Differential GPS */ +} gps_fix_t; + +/** + * @brief GPS fix mode + * + */ +typedef enum { + GPS_MODE_INVALID = 1, /*!< Not fixed */ + GPS_MODE_2D, /*!< 2D GPS */ + GPS_MODE_3D /*!< 3D GPS */ +} gps_fix_mode_t; + +/** + * @brief GPS satellite information + * + */ +typedef struct { + uint8_t num; /*!< Satellite number */ + uint8_t elevation; /*!< Satellite elevation */ + uint16_t azimuth; /*!< Satellite azimuth */ + uint8_t snr; /*!< Satellite signal noise ratio */ +} gps_satellite_t; + +/** + * @brief GPS time + * + */ +typedef struct { + uint8_t hour; /*!< Hour */ + uint8_t minute; /*!< Minute */ + uint8_t second; /*!< Second */ + uint16_t thousand; /*!< Thousand */ +} gps_time_t; + +/** + * @brief GPS date + * + */ +typedef struct { + uint8_t day; /*!< Day (start from 1) */ + uint8_t month; /*!< Month (start from 1) */ + uint16_t year; /*!< Year (start from 2000) */ +} gps_date_t; + +/** + * @brief NMEA Statement + * + */ +typedef enum { + STATEMENT_UNKNOWN = 0, /*!< Unknown statement */ + STATEMENT_GGA, /*!< GGA */ + STATEMENT_GSA, /*!< GSA */ + STATEMENT_RMC, /*!< RMC */ + STATEMENT_GSV, /*!< GSV */ + STATEMENT_GLL, /*!< GLL */ + STATEMENT_VTG /*!< VTG */ +} nmea_statement_t; + +/** + * @brief GPS object + * + */ +typedef struct { + float latitude; /*!< Latitude (degrees) */ + float longitude; /*!< Longitude (degrees) */ + float altitude; /*!< Altitude (meters) */ + gps_fix_t fix; /*!< Fix status */ + uint8_t sats_in_use; /*!< Number of satellites in use */ + gps_time_t tim; /*!< time in UTC */ + gps_fix_mode_t fix_mode; /*!< Fix mode */ + uint8_t sats_id_in_use[GPS_MAX_SATELLITES_IN_USE]; /*!< ID list of satellite in use */ + float dop_h; /*!< Horizontal dilution of precision */ + float dop_p; /*!< Position dilution of precision */ + float dop_v; /*!< Vertical dilution of precision */ + uint8_t sats_in_view; /*!< Number of satellites in view */ + gps_satellite_t sats_desc_in_view[GPS_MAX_SATELLITES_IN_VIEW]; /*!< Information of satellites in view */ + gps_date_t date; /*!< Fix date */ + bool valid; /*!< GPS validity */ + float speed; /*!< Ground speed, unit: m/s */ + float cog; /*!< Course over ground */ + float variation; /*!< Magnetic variation */ +} gps_t; + +/** + * @brief Configuration of NMEA Parser + * + */ +typedef struct { + struct { + uart_port_t uart_port; /*!< UART port number */ + uint32_t rx_pin; /*!< UART Rx Pin number */ + uint32_t baud_rate; /*!< UART baud rate */ + uart_word_length_t data_bits; /*!< UART data bits length */ + uart_parity_t parity; /*!< UART parity */ + uart_stop_bits_t stop_bits; /*!< UART stop bits length */ + uint32_t event_queue_size; /*!< UART event queue size */ + } uart; /*!< UART specific configuration */ +} nmea_parser_config_t; + +/** + * @brief NMEA Parser Handle + * + */ +typedef void *nmea_parser_handle_t; + +/** + * @brief Default configuration for NMEA Parser + * + */ +#define NMEA_PARSER_CONFIG_DEFAULT() \ + { \ + .uart = { \ + .uart_port = UART_NUM_2, \ + .rx_pin = 13, \ + .baud_rate = 9600, \ + .data_bits = UART_DATA_8_BITS, \ + .parity = UART_PARITY_DISABLE, \ + .stop_bits = UART_STOP_BITS_1, \ + .event_queue_size = 16 \ + } \ + } + +/** + * @brief NMEA Parser Event ID + * + */ +typedef enum { + GPS_UPDATE, /*!< GPS information has been updated */ + GPS_UNKNOWN /*!< Unknown statements detected */ +} nmea_event_id_t; + +uart_config_t init_uart_config(); + +/** + * @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); + +/** + * @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); + +/** + * @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); + +/** + * @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); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/src/device/gps.cpp b/src/device/gps.cpp index 30206fb..d1dd51c 100644 --- a/src/device/gps.cpp +++ b/src/device/gps.cpp @@ -2,15 +2,22 @@ #include "device/gpsTestData.h" +#define TIME_ZONE (+1) // Central European Time +#define YEAR_BASE (2022) // date in GPS starts from 2022 +static const char *TAG = "gps_demo"; + TinyGPSPlus gps; HardwareSerial ss(2); // Variables [GPS] -double gpsSpeed = 0.0; // Global. -uint8_t gpsHours; // Global. -uint8_t gpsMinutes; // Global. -uint8_t gpsSeconds; // Global. -double gpsDegree = 0.0; // Global. +float gpsLatitude = 0.0; // GPS Latitude measurement. +float gpsLongitude = 0.0; // GPS Longitude measurement. +float gpsAltitude = 0.0; // GPS Altitude measurement. +double gpsSpeed = 0.0; // Global. +uint8_t gpsHours; // Global. +uint8_t gpsMinutes; // Global. +uint8_t gpsSeconds; // Global. +double gpsDegree = 0.0; // Global. bool gpsActive = false; bool gpsValid = false; @@ -22,67 +29,28 @@ int drawCount = 0; bool isTimeSet = false; void Task_GPS_read_core0(void *pvParameters) { + /* NMEA parser configuration */ + nmea_parser_config_t config = NMEA_PARSER_CONFIG_DEFAULT(); + /* init NMEA parser library */ + nmea_parser_handle_t nmea_hdl = nmea_parser_init(&config); + + nmea_parser_add_handler(nmea_hdl, gps_event_handler, NULL); for (;;) { - while (ss.available() > 0) - gps.encode(ss.read()); - if (gps.location.isUpdated()) - { - ESP_LOGD("GPS_read", "lat/lon:%f/%f, kmph: %f", gps.location.lat(), gps.location.lng(), gps.speed.kmph()); - gpsActive = true; - loopGPSIDX(); - } - gpsDebugCoords(); - vTaskDelay(1000 / portTICK_PERIOD_MS); } } -void initGPS() -{ - ss.begin(GPSBaud, SERIAL_8N1, RXPin, TXPin); -}; - void printGPSInfo() { - gpsSpeed = gps.speed.kmph(); - gpsHours = gps.time.hour(); - gpsMinutes = gps.time.minute(); - gpsSeconds = gps.time.second(); - gpsDegree = gps.course.deg(); - ESP_LOGD("printGPSInfo", "Satellites: %d, hdop: %d, lat/lon:%f/%f, kmph: %f, deg:%f", gps.satellites.value(), gps.hdop.value(), gps.location.lat(), gps.location.lng(), gpsSpeed); + ESP_LOGD("printGPSInfo", "lat/lon:%f/%f, kmph: %f, deg:%f", gpsLatitude, gpsLongitude, gpsSpeed, gpsDegree); char buff[100]; - snprintf(buff, sizeof(buff), "%d: %f, %f, %f, %f", gps.time.value(), gps.location.lat(), gps.location.lng(), gpsSpeed, gps.altitude.meters()); + snprintf(buff, sizeof(buff), "%d:%d:%d %f, %f, %f, %f", M5.Rtc.getTime().hours, M5.Rtc.getTime().minutes, M5.Rtc.getTime().seconds, gpsLatitude, gpsLongitude, gpsSpeed, gpsAltitude); logger(buff, "/gps.csv"); } -void loopGPSIDX() -{ - // Update curr_gps_idx_coords with gps data - calcCoordsToCoordsPxl(curr_gps_pxl_coords, gps.location.lat(), - gps.location.lng(), zoom, tile_size); - - // Update global time variables - gpsHours = gps.time.hour(); - gpsMinutes = gps.time.minute(); - gpsSeconds = gps.time.second(); - gpsSpeed = gps.speed.kmph(); - gpsDegree = gps.course.deg(); - - if (!isTimeSet) - { - M5.Rtc.setDateTime({{static_cast(gps.date.year()), static_cast(gps.date.month()), static_cast(gps.date.day())}, - {static_cast(gps.time.hour()), static_cast(gps.time.minute()), static_cast(gps.time.second())}}); - isTimeSet = true; - } - -#ifdef __loggps__ - printGPSInfo(); // Also Log Data. -#endif -} - void gpsDebugCoords() { // Enter Debugmode if no GPS Signal is received and activated. @@ -101,4 +69,60 @@ void gpsDebugCoords() } } #endif -} \ No newline at end of file +} + +/** + * @brief GPS Event Handler + * + * @param event_handler_arg handler specific arguments + * @param event_base event base, here is fixed to ESP_NMEA_EVENT + * @param event_id event id + * @param event_data event specific arguments + */ +void gps_event_handler(void *event_handler_arg, esp_event_base_t event_base, int32_t event_id, void *event_data) +{ + gps_t *gps = NULL; + switch (event_id) + { + case GPS_UPDATE: + gps = (gps_t *)event_data; + /* print information parsed from GPS statements */ + ESP_LOGI("printGPSInfo", "Satellites: %d, hdop: %d, lat/lon:%f/%f, kmph: %f, deg:%f", gps->sats_in_use, gps->dop_h, gps->latitude, gps->longitude, gps->speed); + + // By getting out the single values we decouple from the tinygpsplus library. + if (gps->fix == GPS_FIX_GPS) + { + calcCoordsToCoordsPxl(curr_gps_pxl_coords, gps->latitude, + gps->longitude, zoom, tile_size); + gpsSpeed = gps->speed; // GPS Speed measurement. + gpsHours = gps->tim.hour; // Time of fix in ms. + gpsMinutes = gps->tim.minute; // Time of fix in ms. + gpsSeconds = gps->tim.second; // Time of fix in ms. + gpsDegree = gps->cog; // GPS Course measurement. + gpsLatitude = gps->latitude; // GPS Latitude measurement. + gpsLongitude = gps->longitude; // GPS Longitude measurement. + gpsAltitude = gps->altitude; + + gpsActive = true; // GPS is active. + gpsValid = true; // GPS is valid (not older than n-Seconds). + + if (!isTimeSet) + { + M5.Rtc.setDateTime({{gps->date.year, gps->date.month, gps->date.day}, + {gps->tim.hour, gps->tim.minute, gps->tim.second}}); + isTimeSet = true; + } +#ifdef __loggps__ + printGPSInfo(); // Also Log Data. +#endif + } + + break; + case GPS_UNKNOWN: + /* print unknown statements */ + // ESP_LOGD(TAG, "Unknown statement:%s", (char *)event_data); + break; + default: + break; + } +} diff --git a/src/device/gps.h b/src/device/gps.h index 974c9fd..3ae3980 100644 --- a/src/device/gps.h +++ b/src/device/gps.h @@ -9,6 +9,8 @@ #include #include +#include "calculate/nmea_parser.h" + extern TinyGPSPlus gps; extern HardwareSerial ss; @@ -18,13 +20,10 @@ static const int8_t RXPin = 13; static const int8_t TXPin = 14; // declared twice, Todo: refactoring needed. -void initGPS(); void Task_GPS_read_core0(void *pvParameters); void gpsDebugCoords(); -void gpsSmartDelay(unsigned long ms); void printGPSInfo(); -void loopGPSIDX(); -void gpsSmartDelay(unsigned long ms); +void gps_event_handler(void *event_handler_arg, esp_event_base_t event_base, int32_t event_id, void *event_data); #endif // GPS_H \ No newline at end of file diff --git a/src/globalVariables.h b/src/globalVariables.h index c341e4d..1c861b5 100644 --- a/src/globalVariables.h +++ b/src/globalVariables.h @@ -17,13 +17,16 @@ extern str_pxl_coords last_drawn_pxl_coords; extern TaskHandle_t Task_GPS_read; // By getting out the single values we decouple from the tinygpsplus library. +extern float gpsLatitude; // GPS Latitude measurement. +extern float gpsLongitude; // GPS Longitude measurement. +extern float gpsAltitude; // GPS Altitude measurement. extern double gpsSpeed; // GPS Speed measurement. extern uint8_t gpsHours; // Time of fix in ms. extern uint8_t gpsMinutes; // Time of fix in ms. extern uint8_t gpsSeconds; // Time of fix in ms. -extern double gpsDegree; // GPS Course measurement. -extern bool gpsActive; // GPS is active. -extern bool gpsValid; // GPS is valid (not older than n-Seconds). +extern double gpsDegree; // GPS Course measurement. +extern bool gpsActive; // GPS is active. +extern bool gpsValid; // GPS is valid (not older than n-Seconds). // Vario variables extern int16_t climb_cms; // Vario climb rate in cm/s. \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 9281836..833a816 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -10,6 +10,8 @@ #include "graphic/drawing.h" // Todo: might bundle the drawing logics in one. #include "graphic/task.h" +#include "calculate/nmea_parser.h" + static constexpr const gpio_num_t SDCARD_CSPIN = GPIO_NUM_4; M5GFX lcd; @@ -31,6 +33,18 @@ void initializeM5Stack() M5.In_I2C.release(); } +void initializeGPS() +{ + /* NMEA parser configuration */ + // nmea_parser_config_t config = NMEA_PARSER_CONFIG_DEFAULT(); + /* init NMEA parser library */ + // nmea_parser_handle_t nmea_hdl = nmea_parser_init(&config); + /* register event handler for NMEA parser library */ + // nmea_parser_add_handler(nmea_hdl, gps_event_handler, NULL); + + // vTaskDelay(10000 / portTICK_PERIOD_MS); +} + void initializeSDCard() { if (!SD.begin(SDCARD_CSPIN, SPI, 20000000)) @@ -94,8 +108,6 @@ void setup() // Initialize SD Card initializeSDCard(); - // Initialize GPS - initGPS(); // Initialize GPS Task initGPSTask(); @@ -105,6 +117,7 @@ void setup() initVarioReadoutTask(); // Initialize Vario Task reading the pressure out (this time scheduled task.) initVarioAverageTask(); + // initializeGPS(); // ESP32 NMEA example. // Initialize direction icon initDirectionIcon();