Allow to configure speed source for DJI OSD

src/main/fc/settings.yaml

@@ -152,6 +152,9 @@ tables:
   - name: djiOsdTempSource
     values: ["ESC", "IMU", "BARO"]
     enum: djiOsdTempSource_e
+  - name: djiOsdSpeedSource
+    values: ["GROUND", "3D", "AIR"]
+    enum: djiOsdSpeedSource_e
   - name: nav_overrides_motor_stop
     enum: navOverridesMotorStop_e
     values: ["OFF_ALWAYS", "OFF", "AUTO_ONLY", "ALL_NAV"]
@@ -3249,3 +3252,9 @@ groups:
           field: esc_temperature_source
           table: djiOsdTempSource
           type: uint8_t
+        - name: dji_speed_source
+          description: "Sets the speed type displayed by the DJI OSD: GROUND, 3D, AIR"
+          default_value: "GROUND"
+          field: speedSource
+          table: djiOsdSpeedSource
+          type: uint8_t

src/main/io/osd.c

@@ -1118,13 +1118,6 @@ 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) {

src/main/io/osd_common.c

@@ -36,6 +36,8 @@
 #include "io/osd_common.h"
 #include "io/osd_grid.h"
 
+#include "navigation/navigation.h"
+
 #if defined(USE_OSD)
 
 #define CANVAS_DEFAULT_GRID_ELEMENT_WIDTH OSD_CHAR_WIDTH
@@ -151,4 +153,13 @@ void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas)
     osdGridDrawSidebars(display);
 }
 
+#ifdef USE_GPS
+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
+
 #endif

src/main/io/osd_common.h

@@ -82,3 +82,8 @@ void osdDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *canvas, c
 // grid slots.
 void osdDrawHeadingGraph(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, int heading);
 void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas);
+
+#ifdef USE_GPS
+int16_t osdGet3DSpeed(void);
+#endif
+

src/main/io/osd_dji_hd.c

@@ -58,6 +58,7 @@
 #include "io/gps.h"
 #include "io/osd.h"
 #include "io/osd_dji_hd.h"
+#include "io/osd_common.h"
 
 #include "rx/rx.h"
 
@@ -68,6 +69,7 @@
 #include "sensors/acceleration.h"
 #include "sensors/esc_sensor.h"
 #include "sensors/temperature.h"
+#include "sensors/pitotmeter.h"
 
 #include "msp/msp.h"
 #include "msp/msp_protocol.h"
@@ -118,11 +120,12 @@
  * but reuse the packet decoder to minimize code duplication
  */
 
-PG_REGISTER_WITH_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig, PG_DJI_OSD_CONFIG, 1);
+PG_REGISTER_WITH_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig, PG_DJI_OSD_CONFIG, 2);
 PG_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig,
     .use_name_for_messages  = true,
     .esc_temperature_source = DJI_OSD_TEMP_ESC,
     .proto_workarounds = DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA,
+    .speedSource = DJI_OSD_SPEED_GROUND,
 );
 
 // External dependency on looptime
@@ -639,13 +642,6 @@ static int32_t osdConvertVelocityToUnit(int32_t vel)
     return -1;
 }
 
-static int16_t osdDJIGet3DSpeed(void)
-{
-    int16_t vert_speed = getEstimatedActualVelocity(Z);
-    int16_t hor_speed = gpsSol.groundSpeed;
-    return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed));
-}
-
 /**
  * Converts velocity into a string based on the current unit system.
  * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds)
@@ -766,7 +762,7 @@ static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name)
                 osdDJIFormatThrottlePosition(djibuf,true);
                 break;
             case 'S':
-                osdDJIFormatVelocityStr(djibuf, osdDJIGet3DSpeed());
+                osdDJIFormatVelocityStr(djibuf, osdGet3DSpeed());
                 break;
             case 'E':
                 osdDJIEfficiencyMahPerKM(djibuf);
@@ -1004,7 +1000,21 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
             sbufWriteU32(dst, gpsSol.llh.lat);
             sbufWriteU32(dst, gpsSol.llh.lon);
             sbufWriteU16(dst, gpsSol.llh.alt / 100);
-            sbufWriteU16(dst, gpsSol.groundSpeed);
+
+            int reportedSpeed = 0;
+            if (djiOsdConfig()->speedSource == DJI_OSD_SPEED_GROUND) {
+                reportedSpeed = gpsSol.groundSpeed;
+            } else if (djiOsdConfig()->speedSource == DJI_OSD_SPEED_3D) {
+                reportedSpeed = osdGet3DSpeed();
+            } else if (djiOsdConfig()->speedSource == DJI_OSD_SPEED_AIR) {
+            #ifdef USE_PITOT
+                reportedSpeed = pitot.airSpeed;
+            #else
+                reportedSpeed = 0;
+            #endif
+            }
+
+            sbufWriteU16(dst, reportedSpeed);
             sbufWriteU16(dst, gpsSol.groundCourse);
             break;
 

src/main/io/osd_dji_hd.h

@@ -68,6 +68,12 @@ enum djiOsdTempSource_e {
     DJI_OSD_TEMP_BARO   = 2
 };
 
+enum djiOsdSpeedSource_e {
+    DJI_OSD_SPEED_GROUND    = 0,
+    DJI_OSD_SPEED_3D        = 1,
+    DJI_OSD_SPEED_AIR       = 2
+};
+
 enum djiOsdProtoWorkarounds_e {
     DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA    = 1 << 0,
 };
@@ -76,6 +82,7 @@ typedef struct djiOsdConfig_s {
     uint8_t use_name_for_messages;
     uint8_t esc_temperature_source;
     uint8_t proto_workarounds;
+    uint8_t speedSource;
 } djiOsdConfig_t;
 
 PG_DECLARE(djiOsdConfig_t, djiOsdConfig);

src/main/programming/logic_condition.c

@@ -44,6 +44,7 @@
 #include "flight/imu.h"
 #include "flight/pid.h"
 #include "drivers/io_port_expander.h"
+#include "io/osd_common.h"
 
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
@@ -402,7 +403,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
 
         //FIXME align with osdGet3DSpeed
         case LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED: // cm/s
-            return (int) sqrtf(sq(gpsSol.groundSpeed) + sq((int)getEstimatedActualVelocity(Z)));
+            return osdGet3DSpeed();
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED: // cm/s