O4 arming workaround

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