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.
103 lines
2.9 KiB
103 lines
2.9 KiB
6 years ago
|
#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
|