diff --git a/libraries/AP_OSD/AP_OSD.cpp b/libraries/AP_OSD/AP_OSD.cpp index eb33a381d3..9b6bde75ca 100644 --- a/libraries/AP_OSD/AP_OSD.cpp +++ b/libraries/AP_OSD/AP_OSD.cpp @@ -33,6 +33,7 @@ #include #include #include +#include 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 diff --git a/libraries/AP_OSD/AP_OSD.h b/libraries/AP_OSD/AP_OSD.h index 1371d713f7..e6bcdd1449 100644 --- a/libraries/AP_OSD/AP_OSD.h +++ b/libraries/AP_OSD/AP_OSD.h @@ -25,6 +25,7 @@ #include #include + #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; diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp index e7e29d7348..4c604900c1 100644 --- a/libraries/AP_OSD/AP_OSD_Screen.cpp +++ b/libraries/AP_OSD/AP_OSD_Screen.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #if APM_BUILD_TYPE(APM_BUILD_Rover) #include #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);