AP_OSD: widgets for BLHeli32 ESC amps, temp, rpm, and GPS lat/long

libraries/AP_OSD/AP_OSD.h

@@ -18,6 +18,7 @@
 
 #include <AP_Param/AP_Param.h>
 #include <AP_Math/AP_Math.h>
+#include <AP_BLHeli/AP_BLHeli.h>
 
 class AP_OSD_Backend;
 
@@ -91,6 +92,15 @@ private:
     AP_OSD_Setting wind{false, 0, 0};
     AP_OSD_Setting aspeed{false, 0, 0};
     AP_OSD_Setting vspeed{false, 0, 0};
+    
+#ifdef HAVE_AP_BLHELI_SUPPORT
+    AP_OSD_Setting blh_temp{false, 0, 0};
+    AP_OSD_Setting blh_rpm{false, 0, 0};
+    AP_OSD_Setting blh_amps{false, 0, 0};
+#endif
+    
+    AP_OSD_Setting gps_latitude{false, 0, 0};
+    AP_OSD_Setting gps_longitude{false, 0, 0};
 
     void draw_altitude(uint8_t x, uint8_t y);
     void draw_bat_volt(uint8_t x, uint8_t y);
@@ -111,6 +121,15 @@ private:
     void draw_vspeed(uint8_t x, uint8_t y);
 
     void draw_speed_vector(uint8_t x, uint8_t y, Vector2f v, int32_t yaw);
+
+#ifdef HAVE_AP_BLHELI_SUPPORT
+    void draw_blh_temp(uint8_t x, uint8_t y);
+    void draw_blh_rpm(uint8_t x, uint8_t y);
+    void draw_blh_amps(uint8_t x, uint8_t y);
+#endif
+    
+    void draw_gps_latitude(uint8_t x, uint8_t y);
+    void draw_gps_longitude(uint8_t x, uint8_t y);
 };
 
 class AP_OSD {
@@ -159,3 +178,4 @@ private:
     bool switch_debouncer;
     uint32_t last_switch_ms;
 };
+

libraries/AP_OSD/AP_OSD_Screen.cpp

@@ -28,6 +28,7 @@
 #include <AP_Math/AP_Math.h>
 #include <AP_RSSI/AP_RSSI.h>
 #include <AP_Notify/AP_Notify.h>
+
 #include <ctype.h>
 #include <GCS_MAVLink/GCS.h>
 
@@ -122,6 +123,28 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
     //@Path: AP_OSD_Setting.cpp
     AP_SUBGROUPINFO(vspeed, "VSPEED", 20, AP_OSD_Screen, AP_OSD_Setting),
 
+#ifdef HAVE_AP_BLHELI_SUPPORT
+    // @Group: BLHTEMP
+    // @Path: AP_OSD_Setting.cpp
+    AP_SUBGROUPINFO(blh_temp, "BLHTEMP", 21, AP_OSD_Screen, AP_OSD_Setting),
+
+    // @Group: BLHRPM 
+    // @Path: AP_OSD_Setting.cpp
+    AP_SUBGROUPINFO(blh_rpm, "BLHRPM", 22, AP_OSD_Screen, AP_OSD_Setting),
+
+    // @Group: BLHAMPS
+    // @Path: AP_OSD_Setting.cpp
+    AP_SUBGROUPINFO(blh_amps, "BLHAMPS", 23, AP_OSD_Screen, AP_OSD_Setting),
+#endif
+    
+    // @Group: GPSLAT
+    // @Path: AP_OSD_Setting.cpp
+    AP_SUBGROUPINFO(gps_latitude, "GPSLAT", 24, AP_OSD_Screen, AP_OSD_Setting),
+
+    // @Group: GPSLONG
+    // @Path: AP_OSD_Setting.cpp
+    AP_SUBGROUPINFO(gps_longitude, "GPSLONG", 25, AP_OSD_Screen, AP_OSD_Setting),
+
     AP_GROUPEND
 };
 
@@ -178,6 +201,11 @@ AP_OSD_Screen::AP_OSD_Screen()
 #define SYM_DOWN         0xA4
 #define SYM_DOWN_DOWN    0xA5
 
+#define SYM_DEGREES_C 0x0E
+#define SYM_DEGREES_F 0x0D
+#define SYM_GPS_LAT   0xA6
+#define SYM_GPS_LONG  0xA7
+
 void AP_OSD_Screen::draw_altitude(uint8_t x, uint8_t y)
 {
     float alt;
@@ -429,6 +457,72 @@ void AP_OSD_Screen::draw_vspeed(uint8_t x, uint8_t y)
     backend->write(x, y, false, "%c%2.0f%c", sym, vspd, SYM_MS);
 }
 
+#ifdef HAVE_AP_BLHELI_SUPPORT
+
+void AP_OSD_Screen::draw_blh_temp(uint8_t x, uint8_t y)
+{
+    AP_BLHeli *blheli = AP_BLHeli::get_singleton();
+    if (blheli) {
+        AP_BLHeli::telem_data td;
+        blheli->get_telem_data(0, td);  // first parameter is index into array of ESC's.  Hardwire to zero (first) for now.
+
+        // AP_BLHeli & blh = AP_BLHeli::AP_BLHeli();
+        uint8_t esc_temp = td.temperature;
+        backend->write(x, y, false, "%3d%c", esc_temp, SYM_DEGREES_C);
+    }
+}
+
+void AP_OSD_Screen::draw_blh_rpm(uint8_t x, uint8_t y)
+{
+    AP_BLHeli *blheli = AP_BLHeli::get_singleton();
+    if (blheli) {
+        AP_BLHeli::telem_data td;
+        blheli->get_telem_data(0, td);  // first parameter is index into array of ESC's.  Hardwire to zero (first) for now.
+
+        int esc_rpm = td.rpm * 14;   // hard-wired assumption for now that motor has 14 poles, so multiply eRPM * 14 to get motor RPM.
+        backend->write(x, y, false, "%5d RPM", esc_rpm);
+    }
+}
+
+void AP_OSD_Screen::draw_blh_amps(uint8_t x, uint8_t y)
+{
+    AP_BLHeli *blheli = AP_BLHeli::get_singleton();
+    if (blheli) {
+        AP_BLHeli::telem_data td;
+        blheli->get_telem_data(0, td);  // first parameter is index into array of ESC's.  Hardwire to zero (first) for now.
+
+        float esc_amps = td.current;
+        backend->write(x, y, false, "%4.1f%c", esc_amps, SYM_AMP);
+    }
+}
+#endif  //HAVE_AP_BLHELI_SUPPORT
+
+void AP_OSD_Screen::draw_gps_latitude(uint8_t x, uint8_t y)
+{
+    AP_GPS & gps = AP::gps();
+    const Location &loc = gps.location();   // loc.lat and loc.lng
+    int32_t dec_portion, frac_portion;
+    int32_t abs_lat = labs(loc.lat);
+
+    dec_portion = abs_lat / 10000000UL;
+    frac_portion = abs_lat - dec_portion*10000000UL;
+
+    backend->write(x, y, false, "%c%3ld.%07ld", SYM_GPS_LAT, (long)dec_portion,(long)frac_portion);
+}
+
+void AP_OSD_Screen::draw_gps_longitude(uint8_t x, uint8_t y)
+{
+    AP_GPS & gps = AP::gps();
+    const Location &loc = gps.location();   // loc.lat and loc.lng
+    int32_t dec_portion, frac_portion;
+    int32_t abs_lon = labs(loc.lng);
+
+    dec_portion = abs_lon / 10000000UL;
+    frac_portion = abs_lon - dec_portion*10000000UL;
+
+    backend->write(x, y, false, "%c%3ld.%07ld", SYM_GPS_LONG, (long)dec_portion,(long)frac_portion);
+}
+
 #define DRAW_SETTING(n) if (n.enabled) draw_ ## n(n.xpos, n.ypos)
 
 void AP_OSD_Screen::draw(void)
@@ -457,4 +551,14 @@ void AP_OSD_Screen::draw(void)
     DRAW_SETTING(heading);
     DRAW_SETTING(wind);
     DRAW_SETTING(home);
+
+#ifdef HAVE_AP_BLHELI_SUPPORT
+    DRAW_SETTING(blh_temp);
+    DRAW_SETTING(blh_rpm);
+    DRAW_SETTING(blh_amps);
+#endif
+    
+    DRAW_SETTING(gps_latitude);
+    DRAW_SETTING(gps_longitude);
 }
+