read mpu in separate thread

main/ugv_io.cc

@@ -18,8 +18,6 @@ static constexpr gpio_num_t       MOTOR_LEFT_DIR  = GPIO_NUM_0;
 static constexpr gpio_num_t       MOTOR_RIGHT_PWM = GPIO_NUM_0;
 static constexpr gpio_num_t       MOTOR_RIGHT_DIR = GPIO_NUM_0;
 
-static const char *TAG = "ugv_io";
-
 IOClass IO;
 
 IOClass::IOClass() {}

main/ugv_io_mpu.cc

@@ -15,6 +15,11 @@ static constexpr float            MPU_MAG_TO_FLUX = (4912.f) / (32760.f);
 
 static const char *TAG = "ugv_io_mpu";
 
+Vec3f::Vec3f() : x(0), y(0), z(0) {}
+Vec3f::Vec3f(float x, float y, float z) : x(x), y(y), z(z) {}
+Vec3f::Vec3f(const mpud::float_axes_t &axes)
+    : x(axes.x), y(axes.y), z(axes.z) {}
+
 MPU::MPU() { mpu_ = new mpud::MPU(); }
 
 MPU::~MPU() { delete mpu_; }
@@ -33,20 +38,46 @@ void MPU::Init() {
   }
   mpu_->setAccelFullScale(MPU_ACCEL_FS);
   mpu_->setGyroFullScale(MPU_GYRO_FS);
-  ESP_LOGI(TAG, "MPU initialized");
+
+  mutex_    = xSemaphoreCreateMutex();
+  mpu_data_ = MpuData{};
+
+  BaseType_t xRet =
+      xTaskCreate(MPU::MPU_Task, "ugv_io_mpu", 2 * 1024, this, 3, &this->task_);
+  if (xRet != pdTRUE) {
+    ESP_LOGE(TAG, "error creating MPU task");
+    return;
+  }
+  ESP_LOGI(TAG, "MPU initialized and task started");
 }
 
 void MPU::GetData(MpuData &data) {
-  esp_err_t ret = mpu_->motion(&accel_, &gyro_, &mag_);
+  xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
+  data = MpuData(mpu_data_);
+  xSemaphoreGive(mutex_);
+}
+
+void MPU::DoTask() {
+  esp_err_t ret;
+  while (true) {
+    vTaskDelay(pdMS_TO_TICKS(50));
+    ret = mpu_->motion(&accel_, &gyro_, &mag_);
     if (ret != ESP_OK) {
       ESP_LOGE(TAG, "error reading MPU");
+      continue;
     }
-  data.accel     = mpud::accelGravity(accel_, MPU_ACCEL_FS);
-  data.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS);
-  data.mag.x     = ((float)mag_.x) * MPU_MAG_TO_FLUX;
-  data.mag.y     = ((float)mag_.y) * MPU_MAG_TO_FLUX;
-  data.mag.z     = ((float)mag_.z) * MPU_MAG_TO_FLUX;
+    xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
+    mpu_data_.accel     = mpud::accelGravity(accel_, MPU_ACCEL_FS);
+    mpu_data_.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS);
+    mpu_data_.mag.x     = ((float)mag_.x) * MPU_MAG_TO_FLUX;
+    mpu_data_.mag.y     = ((float)mag_.y) * MPU_MAG_TO_FLUX;
+    mpu_data_.mag.z     = ((float)mag_.z) * MPU_MAG_TO_FLUX;
+    xSemaphoreGive(mutex_);
+  }
+  vTaskDelete(NULL);
 }
 
+void MPU::MPU_Task(void *arg) { ((MPU *)arg)->DoTask(); }
+
 };  // namespace io
 };  // namespace ugv

main/ugv_io_mpu.hh

@@ -7,15 +7,23 @@
 namespace ugv {
 namespace io {
 
-using mpud::float_axes_t;
+struct Vec3f {
+  float x;
+  float y;
+  float z;
+
+  Vec3f();
+  Vec3f(float x, float y, float z);
+  Vec3f(const mpud::float_axes_t& axes);
+};
 
 struct MpuData {
   // G's
-  float_axes_t accel;
+  Vec3f accel;
   // degrees/s
-  float_axes_t gyro_rate;
+  Vec3f gyro_rate;
   // flux density uT
-  float_axes_t mag;
+  Vec3f mag;
 };
 
 class MPU {
@@ -30,7 +38,14 @@ class MPU {
  private:
   mpud::MPU *      mpu_;
   mpud::raw_axes_t accel_, gyro_, mag_;
+
+  TaskHandle_t task_;
+  SemaphoreHandle_t mutex_;
   MpuData mpu_data_;
+
+  void DoTask();
+
+  static void MPU_Task(void *arg);
 };
 
 }  // namespace io