From bd757b23eacf74c90b7e79dba9bf55cdca44585c Mon Sep 17 00:00:00 2001
From: Louis-Guillaume DUBOIS <contact@lgdubois.fr>
Date: Sat, 2 May 2015 21:18:47 +0200
Subject: [PATCH] adds nav_date displayer to keyboard_cmd

---
 src/display.cpp      | 64 +++++++++++++++++++++++++++++++++++++-------
 src/display.h        |  2 +-
 src/keyboard_cmd.cpp |  7 ++++-
 3 files changed, 61 insertions(+), 12 deletions(-)

diff --git a/src/display.cpp b/src/display.cpp
index 7171c9a..333a258 100644
--- a/src/display.cpp
+++ b/src/display.cpp
@@ -2,7 +2,7 @@
 #include <string>
 #include "display.h"
 
-const int Curses::cmd_kbd_lines = 7;
+const int Curses::cmd_kbd_lines = 8;
 const int Curses::cmd_kbd_columns = 55;
 
 const int Curses::cmd_speed_lines = 4;
@@ -39,12 +39,15 @@ Curses::Curses() {
                       1, cmd_kbd_columns + 1);
   log_line_number = log_sent_w_lines - 2; 
   wattron(log_sent_w, A_BOLD);
-  init_pair(1, COLOR_RED, COLOR_BLACK);
+  init_pair(1, COLOR_GREEN, COLOR_BLACK);
   wattron(log_sent_w, COLOR_PAIR(1));
   scrollok(log_sent_w, TRUE);
 
   nav_data = newwin(nav_data_lines, nav_data_columns,
                     cmd_kbd_lines + get_lines + cmd_speed_lines + 1, 0);
+  init_pair(2, COLOR_RED, COLOR_BLACK);
+  wattron(nav_data, COLOR_PAIR(2));
+  wattron(nav_data, A_BOLD);
 
   print_nav_data();
   print_cmd_kbd();
@@ -70,13 +73,14 @@ char Curses::getchar() {
 
 void Curses::print_cmd_kbd() {
   wmove(cmd_kbd, 0, 0);
-   waddstr(cmd_kbd, "        ---------------------\n");
-   waddstr(cmd_kbd, "takeoff>|  t|⇑ y|↖ u|↑ i|↗ o|\n");
-   waddstr(cmd_kbd, "        |---|---|---|---|---|----\n");
-   waddstr(cmd_kbd, "  reset>|  g|⇐ h|← j|  k|→ l|⇒ m|\n");
-   waddstr(cmd_kbd, "        |---|---|---|---|---|----\n");
-   waddstr(cmd_kbd, "   land>|  b|⇓ n|↙ ,|↓ ;|↘ :|\n");
-   waddstr(cmd_kbd, "        ---------------------\n");
+   waddstr(cmd_kbd, "         ---------------------\n");
+   waddstr(cmd_kbd, " takeoff>|  t|⇑ y|↖ u|↑ i|↗ o|\n");
+   waddstr(cmd_kbd, "         |---|---|---|---|---|----\n");
+   waddstr(cmd_kbd, "   reset>|  g|⇐ h|← j|  k|→ l|⇒ m|\n");
+   waddstr(cmd_kbd, "         |---|---|---|---|---|----\n");
+   waddstr(cmd_kbd, "    land>|  b|⇓ n|↙ ,|↓ ;|↘ :|\n");
+   waddstr(cmd_kbd, "         ---------------------\n");
+   waddstr(cmd_kbd, "        Press ctrl + C to quit");
   wrefresh(cmd_kbd);
 }
 
@@ -126,7 +130,47 @@ void Curses::print_nav_data() {
   wrefresh(nav_data);
 }
 
-void Curses::update_navdata(const float& batteryPercent,
+void Curses::update_nav_data(const float& batteryPercent,
                             const int& state,
                             const float& time) {
+  wmove(nav_data, 0, 10);
+  wprintw(nav_data, "%f %", batteryPercent);
+  wmove(nav_data, 2, 10);
+  wprintw(nav_data, "%f %", time);
+  wmove(nav_data, 1, 10);
+  switch(state) {
+    case 0:
+      waddstr(nav_data, "unknown    ");
+      break;
+    case 1:
+      waddstr(nav_data, "inited     ");
+      break;
+    case 2:
+      waddstr(nav_data, "landed     ");
+      break;
+    case 3:
+      waddstr(nav_data, "flying     ");
+      break;
+    case 4:
+      waddstr(nav_data, "hovering   ");
+      break;
+    case 5:
+      waddstr(nav_data, "test       ");
+      break;
+    case 6:
+      waddstr(nav_data, "taking off ");
+      break;
+    case 7:
+      waddstr(nav_data, "flying     ");
+      break;
+    case 8:
+      waddstr(nav_data, "landing    ");
+      break;
+    case 9:
+      waddstr(nav_data, "looping    ");
+      break;
+    default:
+      ;
+  }
+  wrefresh(nav_data);
 }
diff --git a/src/display.h b/src/display.h
index 7413d10..5afc6e7 100644
--- a/src/display.h
+++ b/src/display.h
@@ -39,7 +39,7 @@ class Curses
 
     // TODO
     void update_cmd_speed(const char& coord, const float& v);
-    void update_navdata(const float& batteryPercent,
+    void update_nav_data(const float& batteryPercent,
                         const int& state,
                         const float& time);
     void log_sent(const std::string& str);
diff --git a/src/keyboard_cmd.cpp b/src/keyboard_cmd.cpp
index e1102ce..e219e5a 100644
--- a/src/keyboard_cmd.cpp
+++ b/src/keyboard_cmd.cpp
@@ -18,7 +18,7 @@ class Callback
       term(terminal) {}
 
     void operator()(const ardrone_autonomy::Navdata::ConstPtr& msg) {
-      term->update_navdata(msg->batteryPercent, msg->state, msg->tm);
+      term->update_nav_data(msg->batteryPercent, msg->state, msg->tm);
     }
 }; // class Callback
 
@@ -56,6 +56,11 @@ class Run
         term->update_cmd_speed('y', y_speed);
         term->update_cmd_speed('z', z_speed);
         term->update_cmd_speed('t', turn);
+
+        float a(0);
+        int s(0);
+        float time(0);
+        term->update_nav_data(a, s, time);
       }
 
     void operator()()