Merge pull request #6610 from iNavFlight/dzikuvx-dji-configurable-speed-source

Allow to configure speed source for DJI OSD

docs/Settings.md

@@ -64,6 +64,7 @@
 | disarm_kill_switch | ON | Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel. |
 | display_force_sw_blink | OFF | OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values are invisible, try setting this to ON |
 | dji_esc_temp_source | ESC | Re-purpose the ESC temperature field for IMU/BARO temperature |
+| dji_speed_source | GROUND | Sets the speed type displayed by the DJI OSD: GROUND, 3D, AIR |
 | dji_use_name_for_messages | ON | Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance |
 | dji_workarounds |  | Enables workarounds for different versions of MSP protocol used |
 | dterm_lpf2_hz | 0 | Cutoff frequency for stage 2 D-term low pass filter |

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"]
@@ -3269,3 +3272,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

@@ -1127,13 +1127,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
@@ -152,3 +154,12 @@ void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas)
 }
 
 #endif
+
+#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

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"
@@ -408,7 +409,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