From f6bfb7669169a4d550113d8faae0ab3eb2c6d0eb Mon Sep 17 00:00:00 2001
From: vierfuffzig <tunella@gmx.de>
Date: Thu, 28 Jun 2018 05:56:00 +0000
Subject: [PATCH] AP_OSD: implement throttle and heading

---
 libraries/AP_OSD/AP_OSD.h          |  4 ++++
 libraries/AP_OSD/AP_OSD_Screen.cpp | 27 ++++++++++++++++++++++++++-
 2 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_OSD/AP_OSD.h b/libraries/AP_OSD/AP_OSD.h
index aad1408592..76356d3b35 100644
--- a/libraries/AP_OSD/AP_OSD.h
+++ b/libraries/AP_OSD/AP_OSD.h
@@ -85,6 +85,8 @@ private:
     AP_OSD_Setting gspeed{false, 0, 0};
     AP_OSD_Setting horizon{true, 15, 8};
     AP_OSD_Setting home{false, 0, 0};
+    AP_OSD_Setting throttle{false, 0, 0};
+    AP_OSD_Setting heading{false, 0, 0};
 
     void draw_altitude(uint8_t x, uint8_t y);
     void draw_bat_volt(uint8_t x, uint8_t y);
@@ -97,6 +99,8 @@ private:
     void draw_gspeed(uint8_t x, uint8_t y);
     void draw_horizon(uint8_t x, uint8_t y);
     void draw_home(uint8_t x, uint8_t y);
+    void draw_throttle(uint8_t x, uint8_t y);
+    void draw_heading(uint8_t x, uint8_t y);
 };
 
 class AP_OSD {
diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp
index fee5cbda07..1f59458b98 100644
--- a/libraries/AP_OSD/AP_OSD_Screen.cpp
+++ b/libraries/AP_OSD/AP_OSD_Screen.cpp
@@ -29,6 +29,7 @@
 #include <AP_RSSI/AP_RSSI.h>
 #include <AP_Notify/AP_Notify.h>
 #include <ctype.h>
+#include <GCS_MAVLink/GCS.h>
 
 const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
 
@@ -88,7 +89,7 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
     // @Group: GSPEED
     // @Path: AP_OSD_Setting.cpp
     AP_SUBGROUPINFO(gspeed, "GSPEED", 12, AP_OSD_Screen, AP_OSD_Setting),
-
+    
     // @Group: HORIZON
     // @Path: AP_OSD_Setting.cpp
     AP_SUBGROUPINFO(horizon, "HORIZON", 13, AP_OSD_Screen, AP_OSD_Setting),
@@ -97,6 +98,14 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
     // @Path: AP_OSD_Setting.cpp
     AP_SUBGROUPINFO(home, "HOME", 14, AP_OSD_Screen, AP_OSD_Setting),
 
+    //@Group: HEADING
+    //@Path: AP_OSD_Setting.cpp
+    AP_SUBGROUPINFO(heading, "HEADING", 15, AP_OSD_Screen, AP_OSD_Setting),
+    
+    //@Group: THROTTLE
+    //@Path: AP_OSD_Setting.cpp
+    AP_SUBGROUPINFO(throttle, "THROTTLE", 16, AP_OSD_Screen, AP_OSD_Setting),
+
     AP_GROUPEND
 };
 
@@ -121,6 +130,8 @@ AP_OSD_Screen::AP_OSD_Screen()
 #define SYM_AMP   0x9A
 #define SYM_MAH   0x07
 #define SYM_KMH   0xA1
+#define SYM_DEGR  0xA8
+#define SYM_PCNT  0x25
 
 #define SYM_SAT_L 0x1E
 #define SYM_SAT_R 0x1F
@@ -301,6 +312,18 @@ void AP_OSD_Screen::draw_home(uint8_t x, uint8_t y)
     }
 }
 
+void AP_OSD_Screen::draw_heading(uint8_t x, uint8_t y)
+{
+    AP_AHRS &ahrs = AP::ahrs();
+    uint16_t yaw = ahrs.yaw_sensor / 100;
+    backend->write(x, y, false, "%3d%c", yaw, SYM_DEGR);
+}
+
+void AP_OSD_Screen::draw_throttle(uint8_t x, uint8_t y)
+{
+    backend->write(x, y, false, "%3d%c", gcs().get_hud_throttle(), SYM_PCNT);
+}
+
 #define DRAW_SETTING(n) if (n.enabled) draw_ ## n(n.xpos, n.ypos)
 
 void AP_OSD_Screen::draw(void)
@@ -322,5 +345,7 @@ void AP_OSD_Screen::draw(void)
     DRAW_SETTING(sats);
     DRAW_SETTING(fltmode);
     DRAW_SETTING(gspeed);
+    DRAW_SETTING(throttle);
+    DRAW_SETTING(heading);
     DRAW_SETTING(home);
 }