AP_OSD: Adding flight distance and summary panels for OSD

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()

libraries/AP_OSD/AP_OSD.cpp

@@ -25,8 +25,9 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_HAL/Util.h>
 #include <RC_Channel/RC_Channel.h>
-
+#include <AP_AHRS/AP_AHRS.h>
 #include <utility>
+#include <AP_Notify/AP_Notify.h>
 
 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()
 {

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;
 };

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);
+
 }