From 3cc840e0eb9f4365432b39fd92b90a9942d7f4d7 Mon Sep 17 00:00:00 2001
From: Alex Mikhalev <alexmikhalevalex@gmail.com>
Date: Thu, 25 Apr 2019 19:45:00 -0700
Subject: [PATCH] re-enable display

---
 main/u8g2_esp32_hal.c |  1 +
 main/ugv_display.cc   | 37 +++++++++++++++++++++----------------
 main/ugv_io_mpu.cc    |  2 +-
 3 files changed, 23 insertions(+), 17 deletions(-)

diff --git a/main/u8g2_esp32_hal.c b/main/u8g2_esp32_hal.c
index 3301e24..4ad4282 100644
--- a/main/u8g2_esp32_hal.c
+++ b/main/u8g2_esp32_hal.c
@@ -124,6 +124,7 @@ uint8_t u8g2_esp32_i2c_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
         break;
       }
 
+      break;
       i2c_config_t conf;
       conf.mode          = I2C_MODE_MASTER;
       conf.sda_io_num    = u8x8->pins[U8X8_PIN_I2C_DATA];
diff --git a/main/ugv_display.cc b/main/ugv_display.cc
index 13769a5..9743253 100644
--- a/main/ugv_display.cc
+++ b/main/ugv_display.cc
@@ -14,12 +14,12 @@ using comms::CommsClass;
 DisplayClass::DisplayClass(CommsClass* comms) : comms_(comms) {}
 
 void DisplayClass::Init() {
-  return;
   ESP_LOGD(TAG, "Initializing u8g2 display");
   // For Heltec ESP32 LoRa
   // oled = new U8G2_SSD1306_128X64_NONAME_F_HW_I2C(U8G2_R0, 16, 15, 4);
   // For wemos Lolin ESP32
-  oled = new U8G2_SSD1306_128X64_NONAME_F_HW_I2C(U8G2_R0, OLED_RESET, OLED_CLOCK, OLED_DATA);
+  oled = new U8G2_SSD1306_128X64_NONAME_F_HW_I2C(U8G2_R0, OLED_RESET,
+                                                 OLED_CLOCK, OLED_DATA);
   oled->initDisplay();
   oled->clearDisplay();
   oled->setPowerSave(false);
@@ -30,28 +30,31 @@ void DisplayClass::Init() {
     ESP_LOGE(TAG, "error starting display thread");
     return;
   }
-  ESP_LOGD(TAG, "Started display thread");
 }
 
 void DisplayClass::Run() {
-  return;
-  int32_t    lora_rssi;
-  uint8_t    lora_lna_gain;
+#ifdef COMMS_SX127X
+  int32_t lora_rssi;
+  uint8_t lora_lna_gain;
+  int32_t last_packet_rssi;
+  int8_t  last_packet_snr;
+#endif
   TickType_t last_packet_tick;
-  int32_t    last_packet_rssi;
-  int8_t     last_packet_snr;
 
+  ESP_LOGD(TAG, "Started display thread");
   while (true) {
-    // ESP_LOGI(TAG, "inputs %s", inputs.ToString());
-
     oled->firstPage();
+#ifdef COMMS_SX127X
     lora_rssi     = comms_->ReadRssi();
     lora_lna_gain = comms_->ReadLnaGain();
+#endif
 
     comms_->Lock();
     last_packet_tick = comms_->last_packet_tick;
+#ifdef COMMS_SX127X
     last_packet_rssi = comms_->last_packet_rssi;
     last_packet_snr  = comms_->last_packet_snr;
+#endif
     comms_->Unlock();
     do {
       oled->drawRFrame(0, 0, OLED_W, OLED_H, 4);
@@ -65,25 +68,27 @@ void DisplayClass::Run() {
       oled->printf("heap allc/free %d/%d", heap_info.total_allocated_bytes,
                    heap_info.total_free_bytes);
 
+#ifdef COMMS_SX127X
       oled->setCursor(4, 3 * 8);
       oled->printf("rssi: %d lna gain: %d", lora_rssi, lora_lna_gain);
+#endif
 
       if (last_packet_tick > 0) {
         double time_since_last_packet =
-            1000.0f /
-            ((xTaskGetTickCount() - last_packet_tick) * portTICK_RATE_MS);
+            static_cast<float>((xTaskGetTickCount() - last_packet_tick) *
+                               portTICK_RATE_MS) /
+            1000.f;
         oled->setCursor(4, 4 * 8);
-        oled->printf("last pkt rx %f s ago", time_since_last_packet);
+        oled->printf("last pkt rx %.2f s ago", time_since_last_packet);
+#ifdef COMMS_SX127X
         oled->setCursor(4, 5 * 8);
         oled->printf("pkt rssi: %d, snr: %f", last_packet_rssi,
                      last_packet_snr * 0.25f);
+#endif
       } else {
         oled->setCursor(4, 4 * 8);
         oled->print("no pkt rx");
       }
-      // oled->setCursor(4, 6 * 8);
-      // oled->printf("motors: (%f, %f)", outputs.left_motor,
-      // outputs.right_motor);
     } while (oled->nextPage());
     vTaskDelay(pdMS_TO_TICKS(1000));
   }
diff --git a/main/ugv_io_mpu.cc b/main/ugv_io_mpu.cc
index b3015a0..17a2bf0 100644
--- a/main/ugv_io_mpu.cc
+++ b/main/ugv_io_mpu.cc
@@ -40,7 +40,7 @@ MPU::~MPU() { delete mpu_; }
 
 void MPU::Init() {
   xSemaphoreTake(i2c_mutex, portMAX_DELAY);
-  mpu_bus_ = &i2c1;
+  mpu_bus_ = &i2c0;
   // This is shared with the oled, so just use those pins
   mpu_bus_->setTimeout(10);
   mpu_bus_->begin(MPU_SDA, MPU_SCL, 400000);