From bea8bf3afb447da0771b31f99ffca4fdf3807264 Mon Sep 17 00:00:00 2001
From: Sebastian Quilter <squilter@mit.edu>
Date: Thu, 11 Feb 2021 08:33:51 -0800
Subject: [PATCH] AP_OSD: Add draw_vtx_power()

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

diff --git a/libraries/AP_OSD/AP_OSD.h b/libraries/AP_OSD/AP_OSD.h
index 9a3764ed4a..1371d713f7 100644
--- a/libraries/AP_OSD/AP_OSD.h
+++ b/libraries/AP_OSD/AP_OSD.h
@@ -178,6 +178,8 @@ private:
     AP_OSD_Setting bat2used{false, 0, 0};
     AP_OSD_Setting current2{false, 0, 0};
     AP_OSD_Setting clk{false, 0, 0};
+    AP_OSD_Setting callsign{false, 0, 0};
+    AP_OSD_Setting vtx_power{false, 0, 0};
 #if HAL_PLUSCODE_ENABLE
     AP_OSD_Setting pluscode{false, 0, 0};
 #endif
@@ -191,7 +193,6 @@ private:
     AP_OSD_Setting cell_volt{true, 1, 1};
     AP_OSD_Setting batt_bar{true, 1, 1};
     AP_OSD_Setting arming{true, 1, 1};
-    AP_OSD_Setting callsign{false, 0, 0};
 
     void draw_altitude(uint8_t x, uint8_t y);
     void draw_bat_volt(uint8_t x, uint8_t y);
@@ -248,6 +249,7 @@ private:
     void draw_clk(uint8_t x, uint8_t y);
     void draw_callsign(uint8_t x, uint8_t y);
     void draw_current2(uint8_t x, uint8_t y);
+    void draw_vtx_power(uint8_t x, uint8_t y);
 
     struct {
         bool load_attempted;
diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp
index a33432410a..e7e29d7348 100644
--- a/libraries/AP_OSD/AP_OSD_Screen.cpp
+++ b/libraries/AP_OSD/AP_OSD_Screen.cpp
@@ -37,6 +37,7 @@
 #include <AP_RTC/AP_RTC.h>
 #include <AP_MSP/msp.h>
 #include <AP_OLC/AP_OLC.h>
+#include <AP_VideoTX/AP_VideoTX.h>
 #if APM_BUILD_TYPE(APM_BUILD_Rover)
 #include <AP_WindVane/AP_WindVane.h>
 #endif
@@ -894,6 +895,22 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
     // @Range: 0 15
     AP_SUBGROUPINFO(current2, "CURRENT2", 54, AP_OSD_Screen, AP_OSD_Setting),
 
+    // @Param: VTX_PWR_EN
+    // @DisplayName: VTX_PWR_EN
+    // @Description: Displays VTX Power
+    // @Values: 0:Disabled,1:Enabled
+
+    // @Param: VTX_PWR_X
+    // @DisplayName: VTX_PWR_X
+    // @Description: Horizontal position on screen
+    // @Range: 0 29
+
+    // @Param: VTX_PWR_Y
+    // @DisplayName: VTX_PWR_Y
+    // @Description: Vertical position on screen
+    // @Range: 0 15
+    AP_SUBGROUPINFO(vtx_power, "VTX_PWR", 55, AP_OSD_Screen, AP_OSD_Setting),
+
     AP_GROUPEND
 };
 
@@ -1821,6 +1838,20 @@ void AP_OSD_Screen::draw_current2(uint8_t x, uint8_t y)
     draw_current(1, x, y);
 }
 
+void AP_OSD_Screen::draw_vtx_power(uint8_t x, uint8_t y)
+{
+    AP_VideoTX *vtx = AP_VideoTX::get_singleton();
+    if (!vtx) {
+        return;
+    }
+    uint16_t powr = 0;
+    // If currently in pit mode, just render 0mW to the screen
+    if(!vtx->has_option(AP_VideoTX::VideoOptions::VTX_PITMODE)){
+        powr = vtx->get_power_mw();
+    }
+    backend->write(x, y, false, "%4hu%c", powr, SYM_MW);
+}
+
 #define DRAW_SETTING(n) if (n.enabled) draw_ ## n(n.xpos, n.ypos)
 
 #if HAL_WITH_OSD_BITMAP
@@ -1864,6 +1895,7 @@ void AP_OSD_Screen::draw(void)
     DRAW_SETTING(hdop);
     DRAW_SETTING(flightime);
     DRAW_SETTING(clk);
+    DRAW_SETTING(vtx_power);
 
 #ifdef HAVE_AP_BLHELI_SUPPORT
     DRAW_SETTING(blh_temp);