From 14e186489611fe575d55a48f7ea9535c02acef06 Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Thu, 24 Jan 2019 18:47:42 -0800 Subject: [PATCH] move display to it's own class and thread --- main/CMakeLists.txt | 1 + main/Print.h | 1 + main/ugv_display.cc | 102 ++++++++++++++++++++++++++++++++++++++++++++ main/ugv_display.hh | 34 +++++++++++++++ main/ugv_main.cc | 90 +++----------------------------------- 5 files changed, 144 insertions(+), 84 deletions(-) create mode 100644 main/ugv_display.cc create mode 100644 main/ugv_display.hh diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 2c8eebc..40cb55c 100644 --- a/main/CMakeLists.txt +++ b/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" diff --git a/main/Print.h b/main/Print.h index fa50071..aacd532 100644 --- a/main/Print.h +++ b/main/Print.h @@ -19,6 +19,7 @@ #include #include +#include #include // #include "Printable.h" diff --git a/main/ugv_display.cc b/main/ugv_display.cc new file mode 100644 index 0000000..4b3ce0e --- /dev/null +++ b/main/ugv_display.cc @@ -0,0 +1,102 @@ +#include "ugv_display.hh" + +#include +#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 diff --git a/main/ugv_display.hh b/main/ugv_display.hh new file mode 100644 index 0000000..53e10d6 --- /dev/null +++ b/main/ugv_display.hh @@ -0,0 +1,34 @@ +#pragma once + +#include +#include + +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 diff --git a/main/ugv_main.cc b/main/ugv_main.cc index ee4533c..0b89e66 100644 --- a/main/ugv_main.cc +++ b/main/ugv_main.cc @@ -2,9 +2,8 @@ #include #include -#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"; - -U8G2 *oled; +DisplayClass *display; -void setup_oled(void) { - // 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); -} +static const char *TAG = "ugv_main"; void setup(void) { ESP_LOGI(TAG, "setup"); - setup_oled(); Comms.Init(); IO.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(); - } + display = new DisplayClass(&Comms, &IO); + display->Init(); } } // namespace ugv extern "C" void app_main() { - xTaskCreatePinnedToCore(ugv::loopTask, "loopTask", 8192, NULL, 1, NULL, 1); + ugv::setup(); }