From c4c8e18f0c7b95568204d15e3ccf7f13ce63cbec Mon Sep 17 00:00:00 2001 From: Hwurzburg Date: Sat, 11 Aug 2018 12:07:31 -0500 Subject: [PATCH] AP_OSD: Adding flight distance and summary panels for OSD --- libraries/AP_HAL/AP_HAL_Macros.h | 1 + libraries/AP_OSD/AP_OSD.cpp | 50 ++++++++++++++++++++++++++++-- libraries/AP_OSD/AP_OSD.h | 15 +++++++-- libraries/AP_OSD/AP_OSD_Screen.cpp | 32 +++++++++++++++++++ 4 files changed, 94 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL/AP_HAL_Macros.h b/libraries/AP_HAL/AP_HAL_Macros.h index 387baee9d9..742096c266 100644 --- a/libraries/AP_HAL/AP_HAL_Macros.h +++ b/libraries/AP_HAL/AP_HAL_Macros.h @@ -48,6 +48,7 @@ #define ceil(x) DO_NOT_USE_DOUBLE_MATHS() #define floor(x) DO_NOT_USE_DOUBLE_MATHS() #define round(x) DO_NOT_USE_DOUBLE_MATHS() +#define fmax(x) DO_NOT_USE_DOUBLE_MATHS() #if !HAL_WITH_UAVCAN // we should do log() and fabs() as well, but can't because of a conflict in uavcan #define log(x) DO_NOT_USE_DOUBLE_MATHS() diff --git a/libraries/AP_OSD/AP_OSD.cpp b/libraries/AP_OSD/AP_OSD.cpp index bbc706ee63..da72b2bb3b 100644 --- a/libraries/AP_OSD/AP_OSD.cpp +++ b/libraries/AP_OSD/AP_OSD.cpp @@ -25,8 +25,9 @@ #include #include #include - +#include #include +#include const AP_Param::GroupInfo AP_OSD::var_info[] = { @@ -199,7 +200,7 @@ void AP_OSD::osd_thread() void AP_OSD::update_osd() { backend->clear(); - + stats(); update_current_screen(); screen[current_screen].set_backend(backend); @@ -208,6 +209,51 @@ void AP_OSD::update_osd() backend->flush(); } +//update maximums and totals +void AP_OSD::stats() +{ + uint32_t now = AP_HAL::millis(); + if (!AP_Notify::flags.armed) { + last_update_ms = now; + return; + } + + // flight distance + uint32_t delta_ms = now - last_update_ms; + last_update_ms = now; + + AP_AHRS &ahrs = AP::ahrs(); + Vector2f v = ahrs.groundspeed_vector(); + float speed = v.length(); + if (speed < 2.0) { + speed = 0.0; + } + float dist_m = (speed * delta_ms)*0.001; + last_distance_m += dist_m; + + // maximum ground speed + max_speed_mps = fmaxf(max_speed_mps,speed); + + // maximum distance + Location loc; + if (ahrs.get_position(loc) && ahrs.home_is_set()) { + const Location &home_loc = ahrs.get_home(); + float distance = get_distance(home_loc, loc); + max_dist_m = fmaxf(max_dist_m, distance); + } + + // maximum altitude + float alt; + AP::ahrs().get_relative_position_D_home(alt); + alt = -alt; + max_alt_m = fmaxf(max_alt_m, alt); + // maximum current + AP_BattMonitor &battery = AP_BattMonitor::battery(); + float amps = battery.current_amps(); + max_current_a = fmaxf(max_current_a, amps); +} + + //Thanks to minimosd authors for the multiple osd screen idea void AP_OSD::update_current_screen() { diff --git a/libraries/AP_OSD/AP_OSD.h b/libraries/AP_OSD/AP_OSD.h index 76687a852d..cb65f206ab 100644 --- a/libraries/AP_OSD/AP_OSD.h +++ b/libraries/AP_OSD/AP_OSD.h @@ -104,6 +104,8 @@ private: AP_OSD_Setting hdop{false, 0, 0}; AP_OSD_Setting waypoint{false, 0, 0}; AP_OSD_Setting xtrack_error{false, 0, 0}; + AP_OSD_Setting dist{false,22,11}; + AP_OSD_Setting stat{false,0,0}; bool check_option(uint32_t option); @@ -156,6 +158,8 @@ private: void draw_hdop(uint8_t x, uint8_t y); void draw_waypoint(uint8_t x, uint8_t y); void draw_xtrack_error(uint8_t x, uint8_t y); + void draw_dist(uint8_t x, uint8_t y); + void draw_stat(uint8_t x, uint8_t y); }; class AP_OSD { @@ -230,15 +234,22 @@ public: private: void osd_thread(); void update_osd(); + void stats(); void update_current_screen(); void next_screen(); AP_OSD_Backend *backend; - uint32_t last_update_ms; - + //variables for screen switching uint8_t current_screen; uint16_t previous_channel_value; bool switch_debouncer; uint32_t last_switch_ms; struct NavInfo nav_info; + + uint32_t last_update_ms; + float last_distance_m; + float max_dist_m; + float max_alt_m; + float max_speed_mps; + float max_current_a; }; diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp index 6ebc7c1791..c4fb188cfd 100644 --- a/libraries/AP_OSD/AP_OSD_Screen.cpp +++ b/libraries/AP_OSD/AP_OSD_Screen.cpp @@ -169,6 +169,15 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = { // @Path: AP_OSD_Setting.cpp AP_SUBGROUPINFO(xtrack_error, "XTRACK", 31, AP_OSD_Screen, AP_OSD_Setting), + // @Group: DIST + // @Path: AP_OSD_Setting.cpp + AP_SUBGROUPINFO(dist, "DIST", 32, AP_OSD_Screen, AP_OSD_Setting), + + // @Group: STATS + // @Path: AP_OSD_Setting.cpp + AP_SUBGROUPINFO(stat, "STATS", 33, AP_OSD_Screen, AP_OSD_Setting), + + AP_GROUPEND }; @@ -259,6 +268,7 @@ AP_OSD_Screen::AP_OSD_Screen() #define SYM_XERR 0xEE #define SYM_KN 0xF0 #define SYM_NM 0xF1 +#define SYM_DIST 0x22 void AP_OSD_Screen::set_backend(AP_OSD_Backend *_backend) { @@ -822,6 +832,25 @@ void AP_OSD_Screen::draw_xtrack_error(uint8_t x, uint8_t y) backend->write(x, y, false, "%c%4d", SYM_XERR, (int)osd->nav_info.wp_xtrack_error); } +void AP_OSD_Screen::draw_stat(uint8_t x, uint8_t y) +{ + backend->write(x+2, y, false, "%c%c%c", 0x4d,0x41,0x58); + backend->write(x, y+1, false, "%c",SYM_GSPD); + backend->write(x+1, y+1, false, "%4d%c", (int)u_scale(SPEED, osd->max_speed_mps), u_icon(SPEED)); + backend->write(x, y+2, false, "%5.1f%c", (double)osd->max_current_a, SYM_AMP); + backend->write(x, y+3, false, "%5d%c", (int)u_scale(ALTITUDE, osd->max_alt_m), u_icon(ALTITUDE)); + backend->write(x, y+4, false, "%c", SYM_HOME); + draw_distance(x+1, y+4, osd->max_dist_m); + backend->write(x, y+5, false, "%c", SYM_DIST); + draw_distance(x+1, y+5, osd->last_distance_m); +} + +void AP_OSD_Screen::draw_dist(uint8_t x, uint8_t y) +{ + backend->write(x, y, false, "%c", SYM_DIST); + draw_distance(x+1, y, osd->last_distance_m); +} + #define DRAW_SETTING(n) if (n.enabled) draw_ ## n(n.xpos, n.ypos) void AP_OSD_Screen::draw(void) @@ -865,5 +894,8 @@ void AP_OSD_Screen::draw(void) DRAW_SETTING(gps_latitude); DRAW_SETTING(gps_longitude); + DRAW_SETTING(dist); + DRAW_SETTING(stat); + }