calibrate MPU and make GPS work

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);
-      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
+    BaseType_t pdRet = xQueueReceive(uart_queue_, &uart_event, portMAX_DELAY);
+    if (!pdRet) {
       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);
 }
 

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

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;

main/ugv_io_mpu.hh

@@ -31,6 +31,7 @@ class MPU {
   ~MPU();
 
   void Init();
+  void Calibrate();
 
   void GetData(MpuData &data);
 

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;
     }