diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c index 08f46491e4..4bf7190eb5 100644 --- a/src/main/io/displayport_msp.c +++ b/src/main/io/displayport_msp.c @@ -47,6 +47,14 @@ static displayPort_t mspDisplayPort; static serialPortIdentifier_e displayPortSerial; +typedef struct displayPortMspCommand_s { + uint8_t command; + uint8_t row; + uint8_t col; + uint8_t attribute; + uint8_t data[OSD_CHAR_BYTES]; +} __attribute__((packed)) displayPortMspCommand_t; + static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *buf, int len) { UNUSED(displayPort); @@ -168,6 +176,44 @@ static uint32_t txBytesFree(const displayPort_t *displayPort) return mspSerialTxBytesFree(); } +#ifdef USE_MSP_DISPLAYPORT_FONT +static bool writeFontCharacter(displayPort_t *displayPort, uint16_t addr, const osdCharacter_t *chr) +{ + displayPortMspCommand_t displayPortCommand; + + if (!chr) { + return false; + } + + displayPortCommand.command = MSP_DP_FONTCHAR_WRITE; + displayPortCommand.row = addr & 0xff; + displayPortCommand.col = (addr >> 8) & 0xff; + displayPortCommand.attribute = 0; + + memcpy(displayPortCommand.data, chr->data, OSD_CHAR_BYTES); + + int res = output(displayPort, MSP_DISPLAYPORT, (uint8_t*)&displayPortCommand, OSD_CHAR_BYTES + 4); + + // 80ms delay needed to ensure the MSP display has enough time to process the font data + delay(80); + + return res > 0; +} + +static bool checkReady(displayPort_t *displayPort, bool rescan) +{ + if ( + !displayPort || + displayPort->deviceType != DISPLAYPORT_DEVICE_TYPE_MSP || + !rescan + ) { + return false; + } + + return true; +} +#endif + static const displayPortVTable_t mspDisplayPortVTable = { .grab = grab, .release = release, @@ -182,6 +228,10 @@ static const displayPortVTable_t mspDisplayPortVTable = { .redraw = redraw, .isSynced = isSynced, .txBytesFree = txBytesFree, +#ifdef USE_MSP_DISPLAYPORT_FONT + .writeFontCharacter = writeFontCharacter, + .checkReady = checkReady, +#endif .layerSupported = NULL, .layerSelect = NULL, .layerCopy = NULL, diff --git a/src/main/io/displayport_msp.h b/src/main/io/displayport_msp.h index 25f624bb08..24456325f6 100644 --- a/src/main/io/displayport_msp.h +++ b/src/main/io/displayport_msp.h @@ -28,13 +28,16 @@ // MSP Display Port commands typedef enum { - MSP_DP_HEARTBEAT = 0, // Release the display after clearing and updating + MSP_DP_HEARTBEAT = 0, // Release the display after clearing and updating MSP_DP_RELEASE = 1, // Release the display after clearing and updating MSP_DP_CLEAR_SCREEN = 2, // Clear the display MSP_DP_WRITE_STRING = 3, // Write a string at given coordinates MSP_DP_DRAW_SCREEN = 4, // Trigger a screen draw MSP_DP_OPTIONS = 5, // Not used by Betaflight. Reserved by Ardupilot and INAV MSP_DP_SYS = 6, // Display system element displayportSystemElement_e at given coordinates +#ifdef USE_MSP_DISPLAYPORT_FONT + MSP_DP_FONTCHAR_WRITE = 7, // New OSD chip works over MSP, enables font write over MSP +#endif MSP_DP_COUNT, } displayportMspCommand_e; diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 15cab1dec6..dc9229c64d 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -947,6 +947,7 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce #define OSD_FLAGS_OSD_HARDWARE_MAX_7456 (1 << 4) #define OSD_FLAGS_OSD_DEVICE_DETECTED (1 << 5) #define OSD_FLAGS_OSD_MSP_DEVICE (1 << 6) +#define OSD_FLAGS_OSD_HARDWARE_AIRBOT_THEIA_OSD (1 << 7) uint8_t osdFlags = 0; @@ -971,7 +972,11 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce break; case OSD_DISPLAYPORT_DEVICE_MSP: - osdFlags |= OSD_FLAGS_OSD_MSP_DEVICE; + osdFlags |= OSD_FLAGS_OSD_MSP_DEVICE +#ifdef USE_MSP_DISPLAYPORT_FONT + | OSD_FLAGS_OSD_HARDWARE_AIRBOT_THEIA_OSD +#endif + ; if (displayIsReady) { osdFlags |= OSD_FLAGS_OSD_DEVICE_DETECTED; }