move display to it's own class and thread

main/CMakeLists.txt

@@ -5,6 +5,7 @@ set(PB_OUT ${CMAKE_CURRENT_SOURCE_DIR}/pb_out)
 set(COMPONENT_SRCS
     "ugv_main.cc"
     "ugv_comms.cc"
+    "ugv_display.cc"
     "ugv_io.cc"
     "ugv_io_gps.cc"
     "ugv_io_mpu.cc"

main/Print.h

@@ -19,6 +19,7 @@
 
 #include <stddef.h>
 #include <stdint.h>
+#include <string.h>
 #include <string>
 
 // #include "Printable.h"

main/ugv_display.cc

@@ -0,0 +1,102 @@
+#include "ugv_display.hh"
+
+#include <esp_log.h>
+#include "U8g2lib.hh"
+#include "ugv_comms.hh"
+#include "ugv_config.h"
+#include "ugv_io.hh"
+
+namespace ugv {
+
+static const char* TAG = "ugv_display";
+
+using comms::CommsClass;
+using io::IOClass;
+
+DisplayClass::DisplayClass(CommsClass* comms, IOClass* io)
+    : comms_(comms), io_(io) {}
+
+void DisplayClass::Init() {
+  // 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, U8X8_PIN_NONE, 4, 5);
+  oled->initDisplay();
+  oled->clearDisplay();
+  oled->setPowerSave(false);
+
+  BaseType_t xRet = xTaskCreate(DisplayClass::RunThread, "ugv_display",
+                                2 * 1024, this, 1, &this->task_handle_);
+  if (xRet != pdTRUE) {
+    ESP_LOGE(TAG, "error starting display thread");
+  }
+}
+
+void DisplayClass::Run() {
+  int32_t    lora_rssi;
+  uint8_t    lora_lna_gain;
+  TickType_t last_packet_tick;
+  int32_t    last_packet_rssi;
+  int8_t     last_packet_snr;
+
+  io::Inputs  inputs;
+  io::Outputs outputs;
+
+  while (true) {
+    io_->ReadInputs(inputs);
+    outputs.left_motor  = outputs.left_motor > 0 ? -0.5 : 1.0;
+    outputs.right_motor = outputs.right_motor > 0 ? -0.5 : 1.0;
+    io_->WriteOutputs(outputs);
+    // ESP_LOGI(TAG, "inputs %s", inputs.ToString());
+
+    oled->firstPage();
+    lora_rssi     = comms_->ReadRssi();
+    lora_lna_gain = comms_->ReadLnaGain();
+
+    comms_->Lock();
+    last_packet_tick = comms_->last_packet_tick;
+    last_packet_rssi = comms_->last_packet_rssi;
+    last_packet_snr  = comms_->last_packet_snr;
+    comms_->Unlock();
+    do {
+      oled->drawRFrame(0, 0, OLED_W, OLED_H, 4);
+
+      multi_heap_info_t heap_info;
+      heap_caps_get_info(&heap_info, MALLOC_CAP_DEFAULT);
+      oled->setFont(u8g2_font_4x6_mr);
+      oled->setCursor(4, 8);
+      oled->print("=====UAS UGV=====");
+      oled->setCursor(4, 2 * 8);
+      oled->printf("heap allc/free %d/%d", heap_info.total_allocated_bytes,
+                   heap_info.total_free_bytes);
+
+      oled->setCursor(4, 3 * 8);
+      oled->printf("rssi: %d lna gain: %d", lora_rssi, lora_lna_gain);
+
+      if (last_packet_tick > 0) {
+        double time_since_last_packet =
+            1000.0f /
+            ((xTaskGetTickCount() - last_packet_tick) * portTICK_RATE_MS);
+        oled->setCursor(4, 4 * 8);
+        oled->printf("last pkt rx %f s ago", time_since_last_packet);
+        oled->setCursor(4, 5 * 8);
+        oled->printf("pkt rssi: %d, snr: %f", last_packet_rssi,
+                     last_packet_snr * 0.25f);
+      } 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));
+  }
+}
+
+void DisplayClass::RunThread(void* arg) {
+  auto display = (DisplayClass*)arg;
+  display->Run();
+  vTaskDelete(NULL);
+}
+
+}  // namespace ugv

main/ugv_display.hh

@@ -0,0 +1,34 @@
+#pragma once
+
+#include <freertos/FreeRTOS.h>
+#include <freertos/task.h>
+
+class U8G2;
+
+namespace ugv {
+
+namespace comms {
+class CommsClass;
+}
+namespace io {
+class IOClass;
+}
+
+class DisplayClass {
+ public:
+  DisplayClass() = delete;
+  DisplayClass(comms::CommsClass *comms, io::IOClass *io);
+
+  void Init();
+
+ private:
+  comms::CommsClass *comms_;
+  io::IOClass *io_;
+  U8G2 *oled;
+  TaskHandle_t task_handle_;
+
+  void Run();
+  static void RunThread(void *arg);
+};
+
+}  // namespace ugv

main/ugv_main.cc

@@ -2,9 +2,8 @@
 #include <string.h>
 #include <u8g2.h>
 
-#include "U8g2lib.hh"
 #include "ugv_comms.hh"
-#include "ugv_config.h"
+#include "ugv_display.hh"
 #include "ugv_io.hh"
 
 namespace ugv {
@@ -12,98 +11,21 @@ namespace ugv {
 using ugv::comms::Comms;
 using ugv::io::IO;
 
-static const char *TAG = "ugv_main";
+DisplayClass *display;
-
-U8G2 *oled;
 
-void setup_oled(void) {
+static const char *TAG = "ugv_main";
-  // 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, U8X8_PIN_NONE, 4, 5);
-  oled->initDisplay();
-  oled->clearDisplay();
-  oled->setPowerSave(false);
-}
 
 void setup(void) {
   ESP_LOGI(TAG, "setup");
 
-  setup_oled();
   Comms.Init();
   IO.Init();
-}
+  display = new DisplayClass(&Comms, &IO);
-
+  display->Init();
-#define BUF_SZ 32
-
-void loop(void) {
-  static int32_t    lora_rssi;
-  static uint8_t    lora_lna_gain;
-  static TickType_t last_packet_tick;
-  static int32_t    last_packet_rssi;
-  static int8_t     last_packet_snr;
-
-  static char        buf[BUF_SZ];
-  static io::Inputs  inputs;
-  static io::Outputs outputs;
-
-  IO.ReadInputs(inputs);
-  outputs.left_motor  = outputs.left_motor > 0 ? -0.5 : 1.0;
-  outputs.right_motor = outputs.right_motor > 0 ? -0.5 : 1.0;
-  IO.WriteOutputs(outputs);
-  // ESP_LOGI(TAG, "inputs %s", inputs.ToString());
-
-  oled->firstPage();
-  lora_rssi     = Comms.ReadRssi();
-  lora_lna_gain = Comms.ReadLnaGain();
-
-  Comms.Lock();
-  last_packet_tick = Comms.last_packet_tick;
-  last_packet_rssi = Comms.last_packet_rssi;
-  last_packet_snr  = Comms.last_packet_snr;
-  Comms.Unlock();
-  do {
-    oled->drawRFrame(0, 0, OLED_W, OLED_H, 4);
-
-    multi_heap_info_t heap_info;
-    heap_caps_get_info(&heap_info, MALLOC_CAP_DEFAULT);
-    oled->setFont(u8g2_font_4x6_mr);
-    oled->drawStr(4, 8, "=====UAS UGV=====");
-    snprintf(buf, BUF_SZ, "heap allc/free %d/%d",
-             heap_info.total_allocated_bytes, heap_info.total_free_bytes);
-    oled->drawStr(4, 2 * 8, buf);
-
-    snprintf(buf, BUF_SZ, "rssi: %d lna gain: %d", lora_rssi, lora_lna_gain);
-    oled->drawStr(4, 3 * 8, buf);
-
-    if (last_packet_tick > 0) {
-      double time_since_last_packet =
-          1000.0f /
-          ((xTaskGetTickCount() - last_packet_tick) * portTICK_RATE_MS);
-      snprintf(buf, BUF_SZ, "last pkt rx %f s ago", time_since_last_packet);
-      oled->drawStr(4, 4 * 8, buf);
-      snprintf(buf, BUF_SZ, "pkt rssi: %d, snr: %f", last_packet_rssi,
-               last_packet_snr * 0.25f);
-      oled->drawStr(4, 5 * 8, buf);
-    } else {
-      oled->drawStr(4, 4 * 8, "no pkt rx");
-    }
-    snprintf(buf, BUF_SZ, "motors: (%f, %f)", outputs.left_motor,
-             outputs.right_motor);
-    oled->drawStr(4, 6 * 8, buf);
-  } while (oled->nextPage());
-  vTaskDelay(pdMS_TO_TICKS(1000));
-}
-
-void loopTask(void *pvUser) {
-  setup();
-  while (1) {
-    loop();
-  }
 }
 
 }  // namespace ugv
 
 extern "C" void app_main() {
-  xTaskCreatePinnedToCore(ugv::loopTask, "loopTask", 8192, NULL, 1, NULL, 1);
+  ugv::setup();
 }