AP_OSD: Add height above terrain panel and warning

libraries/AP_OSD/AP_OSD.cpp

@@ -33,6 +33,7 @@
 #include <AP_BattMonitor/AP_BattMonitor.h>
 #include <utility>
 #include <AP_Notify/AP_Notify.h>
+#include <AP_Terrain/AP_Terrain.h>
 
 const AP_Param::GroupInfo AP_OSD::var_info[] = {
 
@@ -169,8 +170,17 @@ const AP_Param::GroupInfo AP_OSD::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("_BTN_DELAY", 20, AP_OSD, button_delay_ms, 300),
 #endif
-
+#if AP_TERRAIN_AVAILABLE
+    // @Param: _W_TERR
+    // @DisplayName: Terrain warn level
+    // @Description: Set level below which HGT_TER item will flash. -1 disables
+    // @Range: 1 3000
+    // @Units: m
+    // @User: Standard
+    AP_GROUPINFO("_W_TERR", 23, AP_OSD, warn_terr, -1),
 #endif
+
+#endif //osd enabled
 #if OSD_PARAM_ENABLED
     // @Group: 5_
     // @Path: AP_OSD_ParamScreen.cpp

libraries/AP_OSD/AP_OSD.h

@@ -25,6 +25,7 @@
 #include <GCS_MAVLink/GCS.h>
 #include <AP_OLC/AP_OLC.h>
 
+
 #ifndef OSD_ENABLED
 #define OSD_ENABLED !HAL_MINIMIZE_FEATURES
 #endif
@@ -180,6 +181,7 @@ private:
     AP_OSD_Setting clk{false, 0, 0};
     AP_OSD_Setting callsign{false, 0, 0};
     AP_OSD_Setting vtx_power{false, 0, 0};
+    AP_OSD_Setting hgt_abvterr{true, 23, 7};
 #if HAL_PLUSCODE_ENABLE
     AP_OSD_Setting pluscode{false, 0, 0};
 #endif
@@ -218,7 +220,6 @@ private:
 #if HAL_PLUSCODE_ENABLE
     void draw_pluscode(uint8_t x, uint8_t y);
 #endif
-
     //helper functions
     void draw_speed(uint8_t x, uint8_t y, float angle_rad, float magnitude);
     void draw_distance(uint8_t x, uint8_t y, float distance);
@@ -250,6 +251,8 @@ private:
     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);
+    void draw_hgt_abvterr(uint8_t x, uint8_t y);
+
 
     struct {
         bool load_attempted;
@@ -448,6 +451,7 @@ public:
 
     AP_Int8 warn_rssi;
     AP_Int8 warn_nsat;
+    AP_Int32 warn_terr;
     AP_Float warn_batvolt;
     AP_Float warn_bat2volt;
     AP_Int8 msgtime_s;

libraries/AP_OSD/AP_OSD_Screen.cpp

@@ -38,6 +38,7 @@
 #include <AP_MSP/msp.h>
 #include <AP_OLC/AP_OLC.h>
 #include <AP_VideoTX/AP_VideoTX.h>
+#include <AP_Terrain/AP_Terrain.h>
 #if APM_BUILD_TYPE(APM_BUILD_Rover)
 #include <AP_WindVane/AP_WindVane.h>
 #endif
@@ -712,7 +713,7 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
     // @Description: Vertical position on screen
     // @Range: 0 15
     AP_SUBGROUPINFO(clk, "CLK", 43, AP_OSD_Screen, AP_OSD_Setting),
-    
+
 #if HAL_MSP_ENABLED
     // @Param: SIDEBARS_EN
     // @DisplayName: SIDEBARS_EN
@@ -721,7 +722,7 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
 
     // @Param: SIDEBARS_X
     // @DisplayName: SIDEBARS_X
-    // @Description: Horizontal position on screen (MSP OSD only) 
+    // @Description: Horizontal position on screen (MSP OSD only)
     // @Range: 0 29
 
     // @Param: SIDEBARS_Y
@@ -737,7 +738,7 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
 
     // @Param: CRSSHAIR_X
     // @DisplayName: CRSSHAIR_X
-    // @Description: Horizontal position on screen (MSP OSD only) 
+    // @Description: Horizontal position on screen (MSP OSD only)
     // @Range: 0 29
 
     // @Param: CRSSHAIR_Y
@@ -846,7 +847,7 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
 #if HAL_PLUSCODE_ENABLE
     // @Param: PLUSCODE_EN
     // @DisplayName: PLUSCODE_EN
-    // @Description: Displays pluscode (OLC) element 
+    // @Description: Displays pluscode (OLC) element
     // @Values: 0:Disabled,1:Enabled
 
     // @Param: PLUSCODE_X
@@ -911,6 +912,23 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
     // @Range: 0 15
     AP_SUBGROUPINFO(vtx_power, "VTX_PWR", 55, AP_OSD_Screen, AP_OSD_Setting),
 
+#if AP_TERRAIN_AVAILABLE
+    // @Param: TER_HGT_EN
+    // @DisplayName: TER_HGT_EN
+    // @Description: Displays Height above terrain
+    // @Values: 0:Disabled,1:Enabled
+
+    // @Param: TER_HGT_X
+    // @DisplayName: TER_HGT_X
+    // @Description: Horizontal position on screen
+    // @Range: 0 29
+
+    // @Param: TER_HGT_Y
+    // @DisplayName: TER_HGT_Y
+    // @Description: Vertical position on screen
+    // @Range: 0 15
+    AP_SUBGROUPINFO(hgt_abvterr, "TER_HGT", 56, AP_OSD_Screen, AP_OSD_Setting),
+#endif
     AP_GROUPEND
 };
 
@@ -1274,7 +1292,7 @@ void AP_OSD_Screen::draw_speed(uint8_t x, uint8_t y, float angle_rad, float magn
     static const int32_t interval = 36000 / SYM_ARROW_COUNT;
     char arrow = SYM_ARROW_START + ((int32_t(angle_rad*DEGX100) + interval / 2) / interval) % SYM_ARROW_COUNT;
     if (u_scale(SPEED, magnitude) < 10.0) {
-        backend->write(x, y, false, "%c%3.1f%c", arrow, u_scale(SPEED, magnitude), u_icon(SPEED)); 
+        backend->write(x, y, false, "%c%3.1f%c", arrow, u_scale(SPEED, magnitude), u_icon(SPEED));
     } else {
         backend->write(x, y, false, "%c%3d%c", arrow, (int)u_scale(SPEED, magnitude), u_icon(SPEED));
     }
@@ -1851,6 +1869,19 @@ void AP_OSD_Screen::draw_vtx_power(uint8_t x, uint8_t y)
     }
     backend->write(x, y, false, "%4hu%c", powr, SYM_MW);
 }
+#if AP_TERRAIN_AVAILABLE
+void AP_OSD_Screen::draw_hgt_abvterr(uint8_t x, uint8_t y)
+{
+    AP_Terrain &terrain = AP::terrain();
+
+    float terrain_altitude;
+    if (terrain.height_above_terrain(terrain_altitude,true)) {
+        backend->write(x, y, terrain_altitude < osd->warn_terr, "%4d%c", (int)u_scale(ALTITUDE, terrain_altitude), u_icon(ALTITUDE));
+     } else {
+        backend->write(x, y, false, " ---%c", u_icon(ALTITUDE));
+     }
+}
+#endif
 
 #define DRAW_SETTING(n) if (n.enabled) draw_ ## n(n.xpos, n.ypos)
 
@@ -1868,6 +1899,11 @@ void AP_OSD_Screen::draw(void)
     DRAW_SETTING(horizon);
     DRAW_SETTING(compass);
     DRAW_SETTING(altitude);
+
+#if AP_TERRAIN_AVAILABLE
+    DRAW_SETTING(hgt_abvterr);
+#endif
+
     DRAW_SETTING(waypoint);
     DRAW_SETTING(xtrack_error);
     DRAW_SETTING(bat_volt);