From 91b017c27d2eb1ed16a752f08084421851dd84c5 Mon Sep 17 00:00:00 2001
From: Alex Mikhalev <alexmikhalevalex@gmail.com>
Date: Thu, 17 Jan 2019 22:04:30 -0800
Subject: [PATCH] actually configure I2C bus for MPU

---
 main/ugv_io_mpu.cc | 8 +++++++-
 main/ugv_io_mpu.hh | 7 ++++---
 2 files changed, 11 insertions(+), 4 deletions(-)

diff --git a/main/ugv_io_mpu.cc b/main/ugv_io_mpu.cc
index 07ab8ed..774d596 100644
--- a/main/ugv_io_mpu.cc
+++ b/main/ugv_io_mpu.cc
@@ -9,6 +9,8 @@
 namespace ugv {
 namespace io {
 
+static constexpr gpio_num_t       MPU_SDA         = GPIO_NUM_2;
+static constexpr gpio_num_t       MPU_SCL         = GPIO_NUM_3;
 static constexpr mpud::accel_fs_t MPU_ACCEL_FS    = mpud::ACCEL_FS_2G;
 static constexpr mpud::gyro_fs_t  MPU_GYRO_FS     = mpud::GYRO_FS_500DPS;
 static constexpr float            MPU_MAG_TO_FLUX = (4912.f) / (32760.f);
@@ -20,7 +22,7 @@ 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() : mpu_(nullptr), mpu_data_() {}
 
 MPU::~MPU() { delete mpu_; }
 
@@ -28,6 +30,10 @@ void MPU::Init() {
   mutex_    = xSemaphoreCreateMutex();
   mpu_data_ = MpuData{};
 
+  mpu_bus_ = &i2c0;
+  mpu_bus_->begin(MPU_SDA, MPU_SCL);
+  mpu_ = new mpud::MPU(*mpu_bus_);
+
   esp_err_t ret;
   ret = mpu_->testConnection();
   if (ret != ESP_OK) {
diff --git a/main/ugv_io_mpu.hh b/main/ugv_io_mpu.hh
index 8e45402..8d35594 100644
--- a/main/ugv_io_mpu.hh
+++ b/main/ugv_io_mpu.hh
@@ -14,7 +14,7 @@ struct Vec3f {
 
   Vec3f();
   Vec3f(float x, float y, float z);
-  Vec3f(const mpud::float_axes_t& axes);
+  Vec3f(const mpud::float_axes_t &axes);
 };
 
 struct MpuData {
@@ -36,12 +36,13 @@ class MPU {
   void GetData(MpuData &data);
 
  private:
+  mpud::mpu_bus_t *mpu_bus_;
   mpud::MPU *      mpu_;
   mpud::raw_axes_t accel_, gyro_, mag_;
 
-  TaskHandle_t task_;
+  TaskHandle_t      task_;
   SemaphoreHandle_t mutex_;
-  MpuData mpu_data_;
+  MpuData           mpu_data_;
 
   void DoTask();