You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
96 lines
2.8 KiB
96 lines
2.8 KiB
#include "ugv_display.hh" |
|
|
|
#include <esp_log.h> |
|
#include "U8g2lib.hh" |
|
#include "ugv_comms.hh" |
|
#include "ugv_config.h" |
|
|
|
namespace ugv { |
|
|
|
static const char* TAG = "ugv_display"; |
|
|
|
using comms::CommsClass; |
|
|
|
DisplayClass::DisplayClass(CommsClass* comms) : comms_(comms) {} |
|
|
|
void DisplayClass::Init() { |
|
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, U8X8_PIN_NONE, 4, 5); |
|
oled->initDisplay(); |
|
oled->clearDisplay(); |
|
oled->setPowerSave(false); |
|
|
|
BaseType_t xRet = xTaskCreate(DisplayClass::RunThread, "ugv_display", |
|
8 * 1024, this, 1, &this->task_handle_); |
|
if (xRet != pdTRUE) { |
|
ESP_LOGE(TAG, "error starting display thread"); |
|
return; |
|
} |
|
ESP_LOGD(TAG, "Started 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; |
|
|
|
while (true) { |
|
// 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
|
|
|