AP_OSD: add energy item

ArduPilot · Mar 18, 2020 · 3525e4a · 3525e4a
1 parent 1bf0885
commit 3525e4a
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 0 deletions.
diff --git a/libraries/AP_OSD/AP_OSD.h b/libraries/AP_OSD/AP_OSD.h
@@ -124,6 +124,7 @@ class AP_OSD_Screen
     AP_OSD_Setting bat2used{false, 0, 0};
     AP_OSD_Setting clk{false, 0, 0};
     AP_OSD_Setting power{false, 0, 0};
+    AP_OSD_Setting energy{false, 0, 0};
 
     bool check_option(uint32_t option);
 
@@ -145,6 +146,7 @@ class AP_OSD_Screen
     void draw_rssi(uint8_t x, uint8_t y);
     void draw_current(uint8_t x, uint8_t y);
     void draw_power(uint8_t x, uint8_t y);
+    void draw_energy(uint8_t x, uint8_t y);
     void draw_batused(uint8_t x, uint8_t y);
     void draw_batused(uint8_t instance, uint8_t x, uint8_t y);
     void draw_sats(uint8_t x, uint8_t y);

diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp
@@ -721,6 +721,22 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
     // @Range: 0 15
     AP_SUBGROUPINFO(power, "POWER", 44, AP_OSD_Screen, AP_OSD_Setting),
 
+    // @Param: ENERGY_EN
+    // @DisplayName: ENERGY_EN
+    // @Description: Displays main consumed energy
+    // @Values: 0:Disabled,1:Enabled
+
+    // @Param: ENERGY_X
+    // @DisplayName: ENERGY_X
+    // @Description: Horizontal position on screen
+    // @Range: 0 29
+
+    // @Param: ENERGY_Y
+    // @DisplayName: ENERGY_Y
+    // @Description: Vertical position on screen
+    // @Range: 0 15
+    AP_SUBGROUPINFO(energy, "ENERGY", 45, AP_OSD_Screen, AP_OSD_Setting),
+
     AP_GROUPEND
 };
 
@@ -744,6 +760,7 @@ AP_OSD_Screen::AP_OSD_Screen()
 #define SYM_AMP   0x9A
 #define SYM_WATT  0xAE
 #define SYM_MAH   0x07
+#define SYM_WH    0xAB
 #define SYM_MS    0x9F
 #define SYM_FS    0x99
 #define SYM_KMH   0xA1
@@ -995,6 +1012,17 @@ void AP_OSD_Screen::draw_power(uint8_t x, uint8_t y)
     }
 }
 
+void AP_OSD_Screen::draw_energy(uint8_t x, uint8_t y)
+{
+    AP_BattMonitor &battery = AP::battery();
+    float energy_wh;
+    if (!battery.consumed_wh(energy_wh)) {
+        energy_wh = 0;
+    }
+    const char* const fmt = (energy_wh < 10.0 ? "%1.3f%c" : (energy_wh < 100.0 ? "%2.2f%c" : "%3.1f%c"));
+    backend->write(x, y, false, fmt, energy_wh, SYM_WH);
+}
+
 void AP_OSD_Screen::draw_fltmode(uint8_t x, uint8_t y)
 {
     AP_Notify * notify = AP_Notify::get_singleton();
@@ -1612,6 +1640,7 @@ void AP_OSD_Screen::draw(void)
     DRAW_SETTING(rssi);
     DRAW_SETTING(current);
     DRAW_SETTING(power);
+    DRAW_SETTING(energy);
     DRAW_SETTING(batused);
     DRAW_SETTING(bat2used);
     DRAW_SETTING(sats);