From e8ee6cd10706f8da774d8e762226edbe885f0e0f Mon Sep 17 00:00:00 2001
From: Alex Mikhalev <alexmikhalevalex@gmail.com>
Date: Sun, 27 Jan 2019 15:20:08 -0800
Subject: [PATCH] somewhat made mpu + display work together

---
 main/i2c_mutex.h      | 14 ++++++++++++++
 main/u8g2_esp32_hal.c |  3 +++
 main/ugv_io_mpu.cc    |  5 +++++
 main/ugv_main.cc      |  8 +++++++-
 4 files changed, 29 insertions(+), 1 deletion(-)
 create mode 100644 main/i2c_mutex.h

diff --git a/main/i2c_mutex.h b/main/i2c_mutex.h
new file mode 100644
index 0000000..880eb3a
--- /dev/null
+++ b/main/i2c_mutex.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include <freertos/FreeRTOS.h>
+#include <freertos/semphr.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern SemaphoreHandle_t i2c_mutex;
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/main/u8g2_esp32_hal.c b/main/u8g2_esp32_hal.c
index e3661ca..6a640fc 100644
--- a/main/u8g2_esp32_hal.c
+++ b/main/u8g2_esp32_hal.c
@@ -3,6 +3,7 @@
 
 #include "esp_log.h"
 #include "sdkconfig.h"
+#include "i2c_mutex.h"
 
 #include "freertos/FreeRTOS.h"
 #include "freertos/task.h"
@@ -173,8 +174,10 @@ uint8_t u8g2_esp32_i2c_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
       ESP_ERROR_CHECK(
           i2c_master_write(handle_i2c, txbuf, txbuf_ptr - txbuf, ACK_CHECK_EN));
       ESP_ERROR_CHECK(i2c_master_stop(handle_i2c));
+      xSemaphoreTake(i2c_mutex, portMAX_DELAY);
       ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_MASTER_NUM, handle_i2c,
                                            I2C_TIMEOUT_MS / portTICK_RATE_MS));
+      xSemaphoreGive(i2c_mutex);
       i2c_cmd_link_delete(handle_i2c);
       break;
     }
diff --git a/main/ugv_io_mpu.cc b/main/ugv_io_mpu.cc
index 788c2aa..944beb2 100644
--- a/main/ugv_io_mpu.cc
+++ b/main/ugv_io_mpu.cc
@@ -3,6 +3,7 @@
 #include <driver/uart.h>
 #include <esp_log.h>
 
+#include "i2c_mutex.h"
 #include "MPU.hpp"
 #include "mpu/math.hpp"
 
@@ -32,6 +33,7 @@ void MPU::Init() {
 
   mpu_bus_ = &i2c0;
   // This is shared with the oled, so just use those pins
+  xSemaphoreTake(i2c_mutex, portMAX_DELAY);
   mpu_bus_->begin(MPU_SDA, MPU_SCL);
   mpu_ = new mpud::MPU(*mpu_bus_);
 
@@ -54,6 +56,7 @@ void MPU::Init() {
   }
   mpu_->setAccelFullScale(MPU_ACCEL_FS);
   mpu_->setGyroFullScale(MPU_GYRO_FS);
+  xSemaphoreGive(i2c_mutex);
 
   BaseType_t xRet =
       xTaskCreate(MPU::MPU_Task, "ugv_io_mpu", 2 * 1024, this, 3, &this->task_);
@@ -74,7 +77,9 @@ void MPU::DoTask() {
   esp_err_t ret;
   while (true) {
     vTaskDelay(pdMS_TO_TICKS(50));
+    xSemaphoreTake(i2c_mutex, portMAX_DELAY);
     ret = mpu_->motion(&accel_, &gyro_, &mag_);
+    xSemaphoreGive(i2c_mutex);
     if (ret != ESP_OK) {
       ESP_LOGE(TAG, "error reading MPU");
       continue;
diff --git a/main/ugv_main.cc b/main/ugv_main.cc
index d44d4de..c033f08 100644
--- a/main/ugv_main.cc
+++ b/main/ugv_main.cc
@@ -3,6 +3,7 @@
 #include "ugv_comms.hh"
 #include "ugv_display.hh"
 #include "ugv_io.hh"
+#include "i2c_mutex.h"
 
 #include <math.h>
 
@@ -13,6 +14,10 @@ using ugv::io::IOClass;
 
 static const char *TAG = "ugv_main";
 
+extern "C" {
+   SemaphoreHandle_t i2c_mutex;
+}
+
 constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100;
 static const float PI             = atanf(1.0) * 4.0;
 
@@ -36,10 +41,11 @@ struct State {
 
   void Init() {
     esp_timer_init();
+    i2c_mutex = xSemaphoreCreateMutex();
 
     comms->Init();
     io->Init();
-    // display->Init();
+    display->Init();
 
     esp_timer_create_args_t timer_args;
     timer_args.callback        = OnTimeout;