amikhalev/uas-ugv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

get more GPS info and less verbose GPS logging

Browse Source
This commit is contained in:
Alex Mikhalev 2019-04-25 18:25:21 -07:00
parent b55eb69eb8
commit 2ab357b4fc
2 changed files with 30 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
main/ugv_io_gps.cc
View File

@ -37,7 +37,7 @@ const char NMEA_END_CMD[] = "\r\n";
std::string st; std::string st;
static const char *TAG = "ugv_io"; static const char *TAG = "ugv_io_gps";
UART_GPS::UART_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)); xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
data_.valid = rmc.valid; data_.valid = rmc.valid;
data_.latitude = minmea_tofloat(&rmc.latitude); data_.latitude = minmea_tocoord(&rmc.latitude);
data_.longitude = minmea_tofloat(&rmc.longitude); data_.longitude = minmea_tocoord(&rmc.longitude);
data_.speed = minmea_tofloat(&rmc.speed); data_.speed = minmea_tofloat(&rmc.speed);
data_.course = minmea_tofloat(&rmc.course); data_.course = minmea_tofloat(&rmc.course);
data_.last_update = xTaskGetTickCount(); 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_.valid ? "valid" : "invalid", data_.latitude,
data_.longitude, data_.speed, data_.course); data_.longitude, data_.speed, data_.course);
xSemaphoreGive(mutex_); xSemaphoreGive(mutex_);
@ -143,19 +143,35 @@ void UART_GPS::ProcessLine(const char *line, size_t len) {
xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000)); xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
data_.fix_quality = (GpsFixQual)gga.fix_quality; data_.fix_quality = (GpsFixQual)gga.fix_quality;
data_.num_satellites = gga.satellites_tracked; data_.num_satellites = gga.satellites_tracked;
data_.latitude = minmea_tofloat(&gga.latitude); data_.latitude = minmea_tocoord(&gga.latitude);
data_.longitude = minmea_tofloat(&gga.longitude); data_.longitude = minmea_tocoord(&gga.longitude);
if (gga.fix_quality != 0 && gga.altitude_units != 'M') { if (gga.fix_quality != 0 && gga.altitude_units != 'M') {
ESP_LOGW(TAG, "unknown altitude units: %c", gga.altitude_units); ESP_LOGW(TAG, "unknown altitude units: %c", gga.altitude_units);
} }
data_.altitude = minmea_tofloat(&gga.altitude); data_.altitude = minmea_tofloat(&gga.altitude);
data_.last_update = xTaskGetTickCount(); 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_.fix_quality, data_.num_satellites, data_.latitude,
data_.longitude, data_.altitude); data_.longitude, data_.altitude);
xSemaphoreGive(mutex_); xSemaphoreGive(mutex_);
break; 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); default: ESP_LOGV(TAG, "unsupported nmea sentence: %s", line);
} }
} }
@ -184,13 +200,13 @@ void UART_GPS::DoTask() {
esp_err_t ret; esp_err_t ret;
ret = WriteCommand(NMEA_OUTPUT_RMCGGA, sizeof(NMEA_OUTPUT_RMCGGA)); 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)); 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)); vTaskDelay(pdMS_TO_TICKS(100));
ret = WriteCommand(NMEA_Q_RELEASE, sizeof(NMEA_Q_RELEASE)); 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) { while (true) {
BaseType_t pdRet = xQueueReceive(uart_queue_, &uart_event, portMAX_DELAY); BaseType_t pdRet = xQueueReceive(uart_queue_, &uart_event, portMAX_DELAY);
@ -201,7 +217,7 @@ void UART_GPS::DoTask() {
case UART_DATA: break; case UART_DATA: break;
case UART_PATTERN_DET: this->HandleUartPattern(); break; case UART_PATTERN_DET: this->HandleUartPattern(); break;
default: default:
ESP_LOGW(TAG, "Unhandled GPS event type: %d", uart_event.type); ESP_LOGW(TAG, "Unhandled UART event type: %d", uart_event.type);
break; break;
} }
} }

3
main/ugv_io_gps.hh
View File

@ -31,6 +31,9 @@ struct GpsData {
float altitude; // meters float altitude; // meters
float speed; // knots float speed; // knots
float course; // degrees clockwise of north float course; // degrees clockwise of north
float pdop;
float hdop;
float vdop;
}; };
class UART_GPS { class UART_GPS {