From e59e7a3754da421dcb0a821e96f73ad815f6c51a Mon Sep 17 00:00:00 2001
From: Alexander Malishev <malishev@gmail.com>
Date: Wed, 4 Jul 2018 02:23:53 +0400
Subject: [PATCH] AP_OSD: added airspeed and climbrate

---
 libraries/AP_OSD/AP_OSD.h          |  4 +++
 libraries/AP_OSD/AP_OSD_Screen.cpp | 46 ++++++++++++++++++++++++++++--
 2 files changed, 48 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_OSD/AP_OSD.h b/libraries/AP_OSD/AP_OSD.h
index c56bd1b3b1..cc07bbd401 100644
--- a/libraries/AP_OSD/AP_OSD.h
+++ b/libraries/AP_OSD/AP_OSD.h
@@ -89,6 +89,8 @@ private:
     AP_OSD_Setting heading{false, 0, 0};
     AP_OSD_Setting compass{false, 0, 0};
     AP_OSD_Setting wind{false, 0, 0};
+    AP_OSD_Setting aspeed{false, 0, 0};
+    AP_OSD_Setting vspeed{false, 0, 0};
 
     void draw_altitude(uint8_t x, uint8_t y);
     void draw_bat_volt(uint8_t x, uint8_t y);
@@ -105,6 +107,8 @@ private:
     void draw_heading(uint8_t x, uint8_t y);
     void draw_compass(uint8_t x, uint8_t y);
     void draw_wind(uint8_t x, uint8_t y);
+    void draw_aspeed(uint8_t x, uint8_t y);
+    void draw_vspeed(uint8_t x, uint8_t y);
 
     void draw_speed_vector(uint8_t x, uint8_t y, Vector2f v);
 };
diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp
index 5dafb8c08a..dacf50ae96 100644
--- a/libraries/AP_OSD/AP_OSD_Screen.cpp
+++ b/libraries/AP_OSD/AP_OSD_Screen.cpp
@@ -114,6 +114,14 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
     //@Path: AP_OSD_Setting.cpp
     AP_SUBGROUPINFO(wind, "WIND", 18, AP_OSD_Screen, AP_OSD_Setting),
 
+    //@Group: ASPEED
+    //@Path: AP_OSD_Setting.cpp
+    AP_SUBGROUPINFO(wind, "ASPEED", 19, AP_OSD_Screen, AP_OSD_Setting),
+
+    //@Group: VSPEED
+    //@Path: AP_OSD_Setting.cpp
+    AP_SUBGROUPINFO(wind, "VSPEED", 20, AP_OSD_Screen, AP_OSD_Setting),
+
     AP_GROUPEND
 };
 
@@ -137,6 +145,7 @@ AP_OSD_Screen::AP_OSD_Screen()
 #define SYM_VOLT  0x06
 #define SYM_AMP   0x9A
 #define SYM_MAH   0x07
+#define SYM_MS    0x9F
 #define SYM_KMH   0xA1
 #define SYM_DEGR  0xA8
 #define SYM_PCNT  0x25
@@ -164,6 +173,11 @@ AP_OSD_Screen::AP_OSD_Screen()
 #define SYM_HEADING_DIVIDED_LINE  0x1C
 #define SYM_HEADING_LINE          0x1D
 
+#define SYM_UP_UP        0xA3
+#define SYM_UP           0xA4
+#define SYM_DOWN         0xA5
+#define SYM_DOWN_DOWN    0xA6
+
 void AP_OSD_Screen::draw_altitude(uint8_t x, uint8_t y)
 {
     float alt;
@@ -281,7 +295,7 @@ void AP_OSD_Screen::draw_speed_vector(uint8_t x, uint8_t y,Vector2f v)
 void AP_OSD_Screen::draw_gspeed(uint8_t x, uint8_t y)
 {
     Vector2f v = AP::ahrs().groundspeed_vector();
-    backend->write(x, y, false, "V");
+    backend->write(x, y, false, "G");
     draw_speed_vector(x + 1, y, v);
 }
 
@@ -375,7 +389,7 @@ void AP_OSD_Screen::draw_compass(uint8_t x, uint8_t y)
     for (int8_t i=-2; i<=2; i++) {
         int8_t sector = center_sector + i;
         sector = (sector + total_sectors) % total_sectors;
-        backend->write(x + i, y, "%c", compass_circle[sector]);
+        backend->write(x + i, y, false,  "%c", compass_circle[sector]);
     }
 }
 
@@ -386,6 +400,32 @@ void AP_OSD_Screen::draw_wind(uint8_t x, uint8_t y)
     draw_speed_vector(x + 1, y, Vector2f(v.x, v.y));
 }
 
+void AP_OSD_Screen::draw_aspeed(uint8_t x, uint8_t y)
+{
+    float aspd = 0.0f;
+    if (AP::ahrs().airspeed_estimate(&aspd)) {
+        backend->write(x, y, false, "A%3.0f%c", aspd * 3.6, SYM_KMH);
+    }
+}
+
+void AP_OSD_Screen::draw_vspeed(uint8_t x, uint8_t y)
+{
+    Vector3f v;
+    AP::ahrs().get_velocity_NED(v);
+    float vspd = -v.z;
+    char sym;
+    if (vspd > 3.0f) {
+        sym = SYM_UP_UP;
+    } else if (vspd >=0.0f) {
+        sym = SYM_UP;
+    } else if (vspd >= -3.0f) {
+        sym = SYM_DOWN;
+    } else {
+        sym = SYM_DOWN_DOWN;
+    }
+    backend->write(x, y, false, "%c%3.0f%c", sym, vspd, SYM_MS);
+}
+
 #define DRAW_SETTING(n) if (n.enabled) draw_ ## n(n.xpos, n.ypos)
 
 void AP_OSD_Screen::draw(void)
@@ -408,6 +448,8 @@ void AP_OSD_Screen::draw(void)
     DRAW_SETTING(sats);
     DRAW_SETTING(fltmode);
     DRAW_SETTING(gspeed);
+    DRAW_SETTING(aspeed);
+    DRAW_SETTING(vspeed);
     DRAW_SETTING(throttle);
     DRAW_SETTING(heading);
     DRAW_SETTING(wind);