diff --git a/main/ugv_io_gps.cc b/main/ugv_io_gps.cc index 2652778..8517a68 100644 --- a/main/ugv_io_gps.cc +++ b/main/ugv_io_gps.cc @@ -37,7 +37,7 @@ const char NMEA_END_CMD[] = "\r\n"; std::string st; -static const char *TAG = "ugv_io"; +static const char *TAG = "ugv_io_gps"; UART_GPS::UART_GPS() {} @@ -122,12 +122,12 @@ void UART_GPS::ProcessLine(const char *line, size_t len) { } xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000)); data_.valid = rmc.valid; - data_.latitude = minmea_tofloat(&rmc.latitude); - data_.longitude = minmea_tofloat(&rmc.longitude); + data_.latitude = minmea_tocoord(&rmc.latitude); + data_.longitude = minmea_tocoord(&rmc.longitude); data_.speed = minmea_tofloat(&rmc.speed); data_.course = minmea_tofloat(&rmc.course); data_.last_update = xTaskGetTickCount(); - ESP_LOGD(TAG, "RMC: %s, (%f,%f), speed=%f, course=%f", + ESP_LOGV(TAG, "RMC: %s, (%f,%f), speed=%f, course=%f", data_.valid ? "valid" : "invalid", data_.latitude, data_.longitude, data_.speed, data_.course); xSemaphoreGive(mutex_); @@ -143,19 +143,35 @@ void UART_GPS::ProcessLine(const char *line, size_t len) { xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000)); data_.fix_quality = (GpsFixQual)gga.fix_quality; data_.num_satellites = gga.satellites_tracked; - data_.latitude = minmea_tofloat(&gga.latitude); - data_.longitude = minmea_tofloat(&gga.longitude); + data_.latitude = minmea_tocoord(&gga.latitude); + data_.longitude = minmea_tocoord(&gga.longitude); if (gga.fix_quality != 0 && gga.altitude_units != 'M') { ESP_LOGW(TAG, "unknown altitude units: %c", gga.altitude_units); } data_.altitude = minmea_tofloat(&gga.altitude); data_.last_update = xTaskGetTickCount(); - ESP_LOGD(TAG, "GGA: qual: %d, num_sat=%d, (%f,%f), alt=%f", + ESP_LOGV(TAG, "GGA: qual: %d, num_sat=%d, (%f,%f), alt=%f", data_.fix_quality, data_.num_satellites, data_.latitude, data_.longitude, data_.altitude); xSemaphoreGive(mutex_); break; } + case MINMEA_SENTENCE_GSA: { + minmea_sentence_gsa gsa; + bool parse_success; + parse_success = minmea_parse_gsa(&gsa, line); + if (!parse_success) { + goto invalid; + } + xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000)); + data_.pdop = minmea_tofloat(&gsa.pdop); + data_.hdop = minmea_tofloat(&gsa.hdop); + data_.vdop = minmea_tofloat(&gsa.vdop); + ESP_LOGV(TAG, "GSA: pdop=%f, hdop=%f, vdop=%f", + data_.pdop, data_.hdop, data_.vdop); + xSemaphoreGive(mutex_); + break; + } default: ESP_LOGV(TAG, "unsupported nmea sentence: %s", line); } } @@ -184,13 +200,13 @@ void UART_GPS::DoTask() { esp_err_t ret; ret = WriteCommand(NMEA_OUTPUT_RMCGGA, sizeof(NMEA_OUTPUT_RMCGGA)); - ESP_LOGI(TAG, "sent output rmc and gga: %d", ret); + ESP_LOGV(TAG, "sent output rmc and gga: %d", ret); ret = WriteCommand(NMEA_UPDATE_1HZ, sizeof(NMEA_UPDATE_1HZ)); - ESP_LOGI(TAG, "sent update 1hz: %d", ret); + ESP_LOGV(TAG, "sent update 1hz: %d", ret); vTaskDelay(pdMS_TO_TICKS(100)); ret = WriteCommand(NMEA_Q_RELEASE, sizeof(NMEA_Q_RELEASE)); - ESP_LOGI(TAG, "sent q release: %d", ret); + ESP_LOGV(TAG, "sent q release: %d", ret); while (true) { BaseType_t pdRet = xQueueReceive(uart_queue_, &uart_event, portMAX_DELAY); @@ -201,7 +217,7 @@ void UART_GPS::DoTask() { case UART_DATA: break; case UART_PATTERN_DET: this->HandleUartPattern(); break; default: - ESP_LOGW(TAG, "Unhandled GPS event type: %d", uart_event.type); + ESP_LOGW(TAG, "Unhandled UART event type: %d", uart_event.type); break; } } diff --git a/main/ugv_io_gps.hh b/main/ugv_io_gps.hh index 88cb59a..8e72ebf 100644 --- a/main/ugv_io_gps.hh +++ b/main/ugv_io_gps.hh @@ -31,6 +31,9 @@ struct GpsData { float altitude; // meters float speed; // knots float course; // degrees clockwise of north + float pdop; + float hdop; + float vdop; }; class UART_GPS {