diff --git a/main/ugv_io_gps.cc b/main/ugv_io_gps.cc index f33f7c3..2652778 100644 --- a/main/ugv_io_gps.cc +++ b/main/ugv_io_gps.cc @@ -65,14 +65,19 @@ void UART_GPS::Init() { uart_set_pin(GPS_UART, GPS_UART_TX_PIN, GPS_UART_RX_PIN, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE); ret = uart_driver_install(GPS_UART, GPS_UART_RX_BUF_SIZE, - GPS_UART_TX_BUF_SIZE, 0, NULL, 0); + GPS_UART_TX_BUF_SIZE, 8, &this->uart_queue_, 0); if (ret != ESP_OK) { ESP_LOGE(TAG, "uart_driver_install: %d", ret); return; } + uart_enable_pattern_det_intr(GPS_UART, '\n', 1, 10000, 10, 10); + uart_pattern_queue_reset(GPS_UART, 8); + uart_flush(GPS_UART); ESP_LOGI(TAG, "gps uart configured"); - BaseType_t xRet = xTaskCreate(UART_GPS::GPS_Task, "ugv_io_gps", 2 * 1024, + buffer_ = new uint8_t[GPS_BUF_SIZE]; + + BaseType_t xRet = xTaskCreate(UART_GPS::TaskEntry, "ugv_io_gps", 8 * 1024, this, 3, &this->task_); if (xRet != pdTRUE) { ESP_LOGE(TAG, "error creating GPS task"); @@ -102,7 +107,6 @@ err: } void UART_GPS::ProcessLine(const char *line, size_t len) { - ESP_LOGI(TAG, "gps data: %.*s", len, line); enum minmea_sentence_id id = minmea_sentence_id(line, false); switch (id) { case MINMEA_INVALID: @@ -123,7 +127,11 @@ void UART_GPS::ProcessLine(const char *line, size_t len) { 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", + data_.valid ? "valid" : "invalid", data_.latitude, + data_.longitude, data_.speed, data_.course); xSemaphoreGive(mutex_); + break; } case MINMEA_SENTENCE_GGA: { minmea_sentence_gga gga; @@ -137,75 +145,71 @@ void UART_GPS::ProcessLine(const char *line, size_t len) { data_.num_satellites = gga.satellites_tracked; data_.latitude = minmea_tofloat(&gga.latitude); data_.longitude = minmea_tofloat(&gga.longitude); - if (gga.altitude_units != 'M') { + 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", + data_.fix_quality, data_.num_satellites, data_.latitude, + data_.longitude, data_.altitude); xSemaphoreGive(mutex_); + break; } - default: ESP_LOGW(TAG, "unsupported nmea sentence: %s", line); + default: ESP_LOGV(TAG, "unsupported nmea sentence: %s", line); } } -void UART_GPS::GPS_Task(void *arg) { - UART_GPS *gps = (UART_GPS *)arg; - (void)gps; +void UART_GPS::HandleUartPattern() { + int pos = uart_pattern_pop_pos(GPS_UART); + if (pos < 0) { + ESP_LOGE(TAG, "uart_pattern_pop_pos: %d", pos); + return; + } + int read_bytes = + uart_read_bytes(GPS_UART, this->buffer_, pos + 1, pdMS_TO_TICKS(100)); + if (read_bytes <= 0) { + ESP_LOGW(TAG, "uart_read_bytes error: %d", read_bytes); + return; + } + ESP_LOGV(TAG, "GPS bytes received: %.*s", read_bytes, buffer_); + buffer_[read_bytes] = '\0'; + this->ProcessLine((const char *)buffer_, + read_bytes + 1); // process that line +} +void UART_GPS::DoTask() { ESP_LOGI(TAG, "gps_task start"); - char * buf1 = (char *)malloc(GPS_BUF_SIZE); - char * buf2 = (char *)malloc(GPS_BUF_SIZE); - char * buf_end = buf1; - size_t read_bytes; + uart_event_t uart_event; esp_err_t ret; - ret = gps->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); - ret = gps->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); vTaskDelay(pdMS_TO_TICKS(100)); - ret = gps->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); while (true) { - size_t buf_remaining = GPS_BUF_SIZE - (buf_end - buf1); - read_bytes = uart_read_bytes(GPS_UART, (uint8_t *)buf_end, buf_remaining, - portMAX_DELAY); - if (read_bytes <= 0) { - ESP_LOGW(TAG, "GPS error: %d", read_bytes); + BaseType_t pdRet = xQueueReceive(uart_queue_, &uart_event, portMAX_DELAY); + if (!pdRet) { continue; } - ESP_LOGW(TAG, "GPS bytes received: %.*s", read_bytes, buf_end); - buf_end += read_bytes; - int lines_received = 0; - for (char *c = buf1; c < buf_end; c++) { - if (*c != '\n') { // wait for \n, if not found loop again - continue; - } - lines_received++; - c++; // now c is one past the end of the whole string including \n - - size_t remaining = buf_end - c; - memcpy(buf2, c, remaining); // copy remaining text from buf1 to buf2 - buf_end = buf2 + remaining; // update buf_end to point to the end of the - // remaining text in buf2 - - char * str_ptr = buf1; - size_t str_len = c - buf1; - str_ptr[str_len] = - '\0'; // append the NULL byte (safe because old data was copied) - gps->ProcessLine(str_ptr, str_len); // process that line - - std::swap(buf1, buf2); // swap buffers - c = buf1; // start over on text which was copied to buf2 (now buf1) - } - if (lines_received == 0) { - ESP_LOGW(TAG, "no lines received, waiting for more data"); + switch (uart_event.type) { + case UART_DATA: break; + case UART_PATTERN_DET: this->HandleUartPattern(); break; + default: + ESP_LOGW(TAG, "Unhandled GPS event type: %d", uart_event.type); + break; } } - free(buf1); - free(buf2); +} + +void UART_GPS::TaskEntry(void *arg) { + UART_GPS *gps = (UART_GPS *)arg; + gps->DoTask(); vTaskDelete(NULL); } diff --git a/main/ugv_io_gps.hh b/main/ugv_io_gps.hh index 524fe72..88cb59a 100644 --- a/main/ugv_io_gps.hh +++ b/main/ugv_io_gps.hh @@ -45,12 +45,16 @@ class UART_GPS { private: TaskHandle_t task_; SemaphoreHandle_t mutex_; + QueueHandle_t uart_queue_; GpsData data_; + uint8_t* buffer_; esp_err_t WriteCommand(const char* cmd, size_t len); + void HandleUartPattern(); void ProcessLine(const char* line, size_t len); - static void GPS_Task(void* arg); + void DoTask(); + static void TaskEntry(void* arg); }; } // namespace io diff --git a/main/ugv_io_mpu.cc b/main/ugv_io_mpu.cc index faa5863..622bbca 100644 --- a/main/ugv_io_mpu.cc +++ b/main/ugv_io_mpu.cc @@ -51,6 +51,7 @@ void MPU::Init() { ESP_LOGE(TAG, "MPU initialization error"); return; } + Calibrate(); mpu_->setAccelFullScale(MPU_ACCEL_FS); mpu_->setGyroFullScale(MPU_GYRO_FS); // force magnetometer into continuous mode @@ -60,6 +61,19 @@ void MPU::Init() { ESP_LOGI(TAG, "MPU initialized"); } +void MPU::Calibrate() { + mpud::raw_axes_t accel_offset, gyro_offset; + mpu_->computeOffsets(&accel_offset, &gyro_offset); + mpu_->setAccelOffset(accel_offset); + mpu_->setGyroOffset(gyro_offset); + { + auto ao = mpud::math::accelGravity(accel_offset, mpud::ACCEL_FS_2G); + auto go = mpud::math::gyroDegPerSec(gyro_offset, mpud::GYRO_FS_250DPS); + ESP_LOGI(TAG, "MPU offsets: accel=(%f, %f, %f) gyro=(%f, %f, %f)", ao.x, + ao.y, ao.z, go.x, go.y, go.z); + } +} + void MPU::GetData(MpuData &data) { esp_err_t ret; // uint8_t mxh, mxl, myh, myl, mzh, mzl, n; diff --git a/main/ugv_io_mpu.hh b/main/ugv_io_mpu.hh index b41e621..9f63914 100644 --- a/main/ugv_io_mpu.hh +++ b/main/ugv_io_mpu.hh @@ -31,6 +31,7 @@ class MPU { ~MPU(); void Init(); + void Calibrate(); void GetData(MpuData &data); diff --git a/main/ugv_main.cc b/main/ugv_main.cc index 4f0aa2f..47ac6d9 100644 --- a/main/ugv_main.cc +++ b/main/ugv_main.cc @@ -76,13 +76,13 @@ struct State { ahrs_.update(g.x, g.y, g.z, a.x, a.y, a.z, m.x, m.y, m.z); } if (time_us >= last_print + 500 * 1000) { // 1s - ESP_LOGI(TAG, + ESP_LOGD(TAG, "inputs: acc=(%f, %f, %f) gyro=(%f, %f, %f) mag=(%f, %f, %f)", inputs.mpu.accel.x, inputs.mpu.accel.y, inputs.mpu.accel.z, inputs.mpu.gyro_rate.x, inputs.mpu.gyro_rate.y, inputs.mpu.gyro_rate.z, inputs.mpu.mag.x, inputs.mpu.mag.y, inputs.mpu.mag.z); - ESP_LOGI(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", ahrs_.getYaw(), + ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", ahrs_.getYaw(), ahrs_.getPitch(), ahrs_.getRoll()); last_print = time_us; }