From 1e3adb691e5376098731a23412d61d13b5700c27 Mon Sep 17 00:00:00 2001
From: Alex Mikhalev <alexmikhalevalex@gmail.com>
Date: Sat, 1 Jun 2019 17:19:01 -0700
Subject: [PATCH] Show yaw angle on display

---
 main/ugv_display.cc | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/main/ugv_display.cc b/main/ugv_display.cc
index f0a1460..a25a5e3 100644
--- a/main/ugv_display.cc
+++ b/main/ugv_display.cc
@@ -40,7 +40,7 @@ void DisplayClass::Run() {
   int8_t last_packet_snr;
 #endif
   TickType_t last_packet_tick;
-  messages::UGV_State state;
+  messages::UGV_Status status;
 
   ESP_LOGD(TAG, "Started display thread");
   while (true) {
@@ -52,7 +52,7 @@ void DisplayClass::Run() {
 
     comms_->Lock();
     last_packet_tick = comms_->last_packet_tick;
-    state = comms_->status.state();
+    status.CopyFrom(comms_->status);
 #ifdef COMMS_SX127X
     last_packet_rssi = comms_->last_packet_rssi;
     last_packet_snr = comms_->last_packet_snr;
@@ -71,7 +71,7 @@ void DisplayClass::Run() {
                    heap_info.total_free_bytes);
 
       oled->setCursor(4, 3 * 8);
-      oled->printf("state: %d", (int)state);
+      oled->printf("state: %d", status.state());
 
       if (last_packet_tick > 0) {
         double time_since_last_packet =
@@ -89,6 +89,8 @@ void DisplayClass::Run() {
         oled->setCursor(4, 4 * 8);
         oled->print("no pkt rx");
       }
+      oled->setCursor(4, 5 * 8);
+      oled->printf("yaw angle: %f", status.yaw_angle());
     } while (oled->nextPage());
     vTaskDelay(pdMS_TO_TICKS(1000));
   }