DJI Strikes again

The O4 firmware currently shipping has a regression, when compared to
DJI O3.

They are not accepting INAV's reply to STATUS/STATUS_EX commands.

This patch adds the option to re-use the workaround needed for the
Original DJI FPV System, where INAV would reply with a custom message to
those messages.

The main difference is related to the number of flight modes in INAV vs
Betaflight. INAV has a lot more modes and is likely sending a reply that
DJI thinks is too big and marked as invalid.

While the arming bit is supposed to be the same accross BF and INAV,
they are also not using the correct workflow of asking the firmware for
the list of modes supported (and corresponding bits) to check what is
active or not.

While it is not critical to check the modes, as it is very unlikely the
ARM bit will move/change, it is certainly incorrect to ignore the reply
based on the number of modes, if you haven't checked how many modes
there should be.

docs/Settings.md

@@ -842,6 +842,16 @@ Q factor for dynamic notches
 
 ---
 
+### enable_broken_o4_workaround
+
+DJI O4 release firmware has a broken MSP DisplayPort implementation. This enables a workaround to restore ARM detection.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
 ### esc_sensor_listen_only
 
 Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case

src/main/fc/settings.yaml

@@ -3776,6 +3776,12 @@ groups:
         field: highlight_djis_missing_characters
         default_value: ON
         type: bool
+      - name: enable_broken_o4_workaround
+        field: enable_broken_o4_workaround
+        description: DJI O4 release firmware has a broken MSP DisplayPort implementation. This enables a workaround to restore ARM detection.
+        condition: USE_DJI_HD_OSD
+        default_value: OFF
+        type: bool
       - name: osd_switch_indicator_zero_name
         description: "Character to use for OSD switch incicator 0."
         field: osd_switch_indicator0_name

src/main/io/displayport_msp_osd.c

@@ -53,6 +53,12 @@
 #include "displayport_msp_osd.h"
 #include "displayport_msp_dji_compat.h"
 
+#include "osd_dji_hd.h"
+#include "fc/fc_msp_box.h"
+#include "scheduler/scheduler.h"
+#include "fc/config.h"
+#include "common/maths.h"
+
 #define FONT_VERSION 3
 
 typedef enum {          // defines are from hdzero code
@@ -531,11 +537,72 @@ static mspResult_e processMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPo
     return mspProcessCommand(cmd, reply, mspPostProcessFn);
 }
 
+#if defined(USE_OSD) && defined(USE_DJI_HD_OSD)
+extern timeDelta_t cycleTime;
+static mspResult_e fixDjiBrokenO4ProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) {
+    UNUSED(mspPostProcessFn);
+
+    sbuf_t *dst = &reply->buf;
+
+    // If users is using a buggy O4 air unit, re-use the OLD DJI FPV system workaround for status messages
+    if (osdConfig()->enable_broken_o4_workaround && ((cmd->cmd == DJI_MSP_STATUS) || (cmd->cmd == DJI_MSP_STATUS_EX))) {
+        // Start initializing the reply message
+        reply->cmd = cmd->cmd;
+        reply->result = MSP_RESULT_ACK;
+
+        // DJI OSD relies on a statically defined bit order and doesn't use
+        // MSP_BOXIDS to get actual BOX order. We need a special
+        // packBoxModeFlags()
+        // This is a regression from O3
+        boxBitmask_t flightModeBitmask;
+        djiPackBoxModeBitmask(&flightModeBitmask);
+
+        sbufWriteU16(dst, (uint16_t)cycleTime);
+        sbufWriteU16(dst, 0);
+        sbufWriteU16(dst, packSensorStatus());
+        sbufWriteData(dst, &flightModeBitmask,
+                      4);  // unconditional part of flags, first 32 bits
+        sbufWriteU8(dst, getConfigProfile());
+
+        sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
+        if (cmd->cmd == MSP_STATUS_EX) {
+            sbufWriteU8(dst, 3);  // PID_PROFILE_COUNT
+            sbufWriteU8(dst, 1);  // getCurrentControlRateProfileIndex()
+        } else {
+            sbufWriteU16(dst, cycleTime);  // gyro cycle time
+        }
+
+        // Cap BoxModeFlags to 32 bits
+        // write flightModeFlags header. Lowest 4 bits contain number of bytes
+        // that follow
+        sbufWriteU8(dst, 0);
+        // sbufWriteData(dst, ((uint8_t*)&flightModeBitmask) + 4, byteCount);
+
+        // Write arming disable flags
+        sbufWriteU8(dst, DJI_ARMING_DISABLE_FLAGS_COUNT);
+        sbufWriteU32(dst, djiPackArmingDisabledFlags());
+
+        // Extra flags
+        sbufWriteU8(dst, 0);
+        // Process DONT_REPLY flag
+        if (cmd->flags & MSP_FLAG_DONT_REPLY) {
+            reply->result = MSP_RESULT_NO_REPLY;
+        }
+
+        return reply->result;
+    }
+
+    return processMspCommand(cmd, reply, mspPostProcessFn);
+}
+#else
+#define fixDjiBrokenO4ProcessMspCommand processMspCommand 
+#endif
+
 void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn)
 {
     if (mspPort.port) {
         mspProcessCommand = mspProcessCommandFn;
-        mspSerialProcessOnePort(&mspPort, MSP_SKIP_NON_MSP_DATA, processMspCommand);
+        mspSerialProcessOnePort(&mspPort, MSP_SKIP_NON_MSP_DATA, fixDjiBrokenO4ProcessMspCommand);
     }
 }
 

src/main/io/osd.c

@@ -224,7 +224,7 @@ static bool osdDisplayHasCanvas;
 
 #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
 
-PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 14);
+PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 15);
 PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 3);
 
 void osdStartedSaveProcess(void) {

src/main/io/osd.h

@@ -489,6 +489,7 @@ typedef struct osdConfig_s {
 #ifndef DISABLE_MSP_DJI_COMPAT
     bool            highlight_djis_missing_characters;  // If enabled, show question marks where there is no character in DJI's font to represent an OSD element symbol
 #endif
+    bool            enable_broken_o4_workaround;        // If enabled, override STATUS/STATUS_EX messages to work around DJI's broken O4 air unit MSP DisplayPort implementation
 #ifdef USE_ADSB
     uint16_t adsb_distance_warning;                     // in metres
     uint16_t adsb_distance_alert;                       // in metres

src/main/io/osd_dji_hd.c

@@ -89,13 +89,6 @@
 
 #if defined(USE_DJI_HD_OSD)
 
-#define DJI_MSP_BAUDRATE                    115200
-
-#define DJI_ARMING_DISABLE_FLAGS_COUNT      25
-#define DJI_OSD_WARNING_COUNT               16
-#define DJI_OSD_TIMER_COUNT                 2
-#define DJI_OSD_FLAGS_OSD_FEATURE           (1 << 0)
-#define EFFICIENCY_UPDATE_INTERVAL          (5 * 1000)
 
 #define RC_RX_LINK_LOST_MSG "!RC RX LINK LOST!"
 
@@ -269,7 +262,7 @@ void djiOsdSerialInit(void)
     }
 }
 
-static void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask)
+void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask)
 {
     memset(flightModeBitmask, 0, sizeof(boxBitmask_t));
 
@@ -311,7 +304,7 @@ static void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask)
     }
 }
 
-static uint32_t djiPackArmingDisabledFlags(void)
+uint32_t djiPackArmingDisabledFlags(void)
 {
     // TODO: Map INAV arming disabled flags to DJI/BF ones
     // https://github.com/betaflight/betaflight/blob/c6e5882dd91fa20d246b8f8af10cf6c92876bc3d/src/main/fc/runtime_config.h#L42

src/main/io/osd_dji_hd.h

@@ -30,6 +30,7 @@
 #include "msp/msp_serial.h"
 
 #include "config/parameter_group.h"
+#include "fc/rc_modes.h"
 
 #if defined(USE_DJI_HD_OSD)
 
@@ -66,6 +67,14 @@
 #define DJI_ALTERNATING_DURATION_LONG   (djiOsdConfig()->craftNameAlternatingDuration * 100)
 #define DJI_ALTERNATING_DURATION_SHORT  1000
 
+#define DJI_MSP_BAUDRATE                    115200
+
+#define DJI_ARMING_DISABLE_FLAGS_COUNT      25
+#define DJI_OSD_WARNING_COUNT               16
+#define DJI_OSD_TIMER_COUNT                 2
+#define DJI_OSD_FLAGS_OSD_FEATURE           (1 << 0)
+#define EFFICIENCY_UPDATE_INTERVAL          (5 * 1000)
+
 enum djiOsdTempSource_e {
     DJI_OSD_TEMP_ESC    = 0,
     DJI_OSD_TEMP_CORE   = 1,
@@ -95,4 +104,7 @@ PG_DECLARE(djiOsdConfig_t, djiOsdConfig);
 void djiOsdSerialInit(void);
 void djiOsdSerialProcess(void);
 
+uint32_t djiPackArmingDisabledFlags(void);
+void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask);
+
 #endif