Merge branch 'development' into osd_temperature

iNavFlight · Nov 18, 2018 · 96a8cd2 · 96a8cd2
2 parents ae85fea + af0102c
commit 96a8cd2
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 1 deletion.
diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c
@@ -187,6 +187,7 @@ static const OSD_Entry menuOsdElemsEntries[] =
     OSD_ELEMENT_ENTRY("GPS LAT", OSD_GPS_LAT),
     OSD_ELEMENT_ENTRY("GPS LON", OSD_GPS_LON),
     OSD_ELEMENT_ENTRY("GPS HDOP", OSD_GPS_HDOP),
+    OSD_ELEMENT_ENTRY("3D SPEED", OSD_3D_SPEED),
 #endif // GPS
     OSD_ELEMENT_ENTRY("HEADING", OSD_HEADING),
     OSD_ELEMENT_ENTRY("HEADING GR.", OSD_HEADING_GRAPH),

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
@@ -1078,6 +1078,13 @@ static void osdDrawRadar(uint16_t *drawn, uint32_t *usedScale)
     osdDrawMap(reference, 0, SYM_ARROW_UP, GPS_distanceToHome, poiDirection, SYM_HOME, drawn, usedScale);
 }
 
+static int16_t osdGet3DSpeed(void)
+{
+    int16_t vert_speed = getEstimatedActualVelocity(Z);
+    int16_t hor_speed = gpsSol.groundSpeed;
+    return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed));
+}
+
 #endif
 
 static void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController, uint8_t scale, bool showDecimal) {
@@ -1244,6 +1251,12 @@ static bool osdDrawSingleElement(uint8_t item)
         osdFormatVelocityStr(buff, gpsSol.groundSpeed);
         break;
 
+    case OSD_3D_SPEED:
+        {
+            osdFormatVelocityStr(buff, osdGet3DSpeed());
+            break;
+        }
+
     case OSD_GPS_LAT:
         osdFormatCoordinate(buff, SYM_LAT, gpsSol.llh.lat);
         break;
@@ -2357,6 +2370,10 @@ static uint8_t osdIncElementIndex(uint8_t elementIndex)
         if (elementIndex == OSD_MAP_NORTH) {
             elementIndex = OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE;
         }
+        if (elementIndex == OSD_3D_SPEED) {
+            elementIndex++;
+        }
+
     }
 
     if (elementIndex == OSD_ITEM_COUNT) {
@@ -2388,6 +2405,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
     osdConfig->item_pos[0][OSD_MAIN_BATT_CELL_VOLTAGE] = OSD_POS(12, 1);
     osdConfig->item_pos[0][OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE] = OSD_POS(12, 1);
     osdConfig->item_pos[0][OSD_GPS_SPEED] = OSD_POS(23, 1);
+    osdConfig->item_pos[0][OSD_3D_SPEED] = OSD_POS(23, 1);
 
     osdConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG;
     osdConfig->item_pos[0][OSD_THROTTLE_POS_AUTO_THR] = OSD_POS(6, 2);
@@ -2595,7 +2613,7 @@ static void osdUpdateStats(void)
     int16_t value;
 
     if (feature(FEATURE_GPS)) {
-        value = gpsSol.groundSpeed;
+        value = osdGet3DSpeed();
         if (stats.max_speed < value)
             stats.max_speed = value;
 

diff --git a/src/main/io/osd.h b/src/main/io/osd.h
@@ -121,6 +121,7 @@ typedef enum {
     OSD_MC_VEL_Y_PID_OUTPUTS,
     OSD_MC_VEL_Z_PID_OUTPUTS,
     OSD_MC_POS_XYZ_P_OUTPUTS,
+    OSD_3D_SPEED,
     OSD_TEMPERATURE,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;