diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c index ca122435ee..b26838225a 100644 --- a/src/main/drivers/display.c +++ b/src/main/drivers/display.c @@ -54,12 +54,10 @@ static bool displayAttributesRequireEmulation(displayPort_t *instance, textAttri } static bool displayEmulateTextAttributes(displayPort_t *instance, - char *buf, - const char *s, size_t length, + char *buf, size_t length, textAttributes_t *attr) { UNUSED(instance); - UNUSED(s); // We only emulate blink for now, so there's no need to test // for it again. @@ -148,7 +146,7 @@ int displayWriteWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, const ch // We can't overwrite s, so we use an intermediate buffer if we need // text attribute emulation. size_t blockSize = length > sizeof(buf) ? sizeof(buf) : length; - if (displayEmulateTextAttributes(instance, buf, s, blockSize, &attr)) { + if (displayEmulateTextAttributes(instance, buf, blockSize, &attr)) { // Emulation required rewriting the string, use buf. s = buf; } @@ -157,26 +155,35 @@ int displayWriteWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, const ch return instance->vTable->writeString(instance, x, y, s, attr); } -int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t c) +int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c) { + if (c > instance->maxChar) { + return -1; + } instance->posX = x + 1; instance->posY = y; return instance->vTable->writeChar(instance, x, y, c, TEXT_ATTRIBUTES_NONE); } -int displayWriteCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t c, textAttributes_t attr) +int displayWriteCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c, textAttributes_t attr) { + if (c > instance->maxChar) { + return -1; + } if (displayAttributesRequireEmulation(instance, attr)) { - displayEmulateTextAttributes(instance, (char *)&c, (char *)&c, 1, &attr); + char ec; + if (displayEmulateTextAttributes(instance, &ec, 1, &attr)) { + c = ec; + } } instance->posX = x + 1; instance->posY = y; return instance->vTable->writeChar(instance, x, y, c, attr); } -bool displayReadCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t *c, textAttributes_t *attr) +bool displayReadCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t *c, textAttributes_t *attr) { - uint8_t dc; + uint16_t dc; textAttributes_t dattr; if (!instance->vTable->readChar) { @@ -214,6 +221,14 @@ uint16_t displayTxBytesFree(const displayPort_t *instance) return instance->vTable->txBytesFree(instance); } +bool displayGetFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *instance) +{ + if (instance->vTable->getFontMetadata) { + return instance->vTable->getFontMetadata(metadata, instance); + } + return false; +} + void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable) { instance->vTable = vTable; @@ -228,5 +243,13 @@ void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable) if (displayConfig()->force_sw_blink) { TEXT_ATTRIBUTES_REMOVE_BLINK(instance->cachedSupportedTextAttributes); } + + displayFontMetadata_t metadata; + if (displayGetFontMetadata(&metadata, instance)) { + instance->maxChar = metadata.charCount - 1; + } else { + // Assume 8-bit character implementation + instance->maxChar = 255; + } } diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h index c061611792..4b76a6349a 100644 --- a/src/main/drivers/display.h +++ b/src/main/drivers/display.h @@ -54,6 +54,11 @@ typedef uint8_t textAttributes_t; static inline void TEXT_ATTRIBUTES_COPY(textAttributes_t *dst, textAttributes_t *src) { *dst = *src; } +typedef struct displayFontMetadata_s { + uint8_t version; + uint16_t charCount; +} displayFontMetadata_t; + struct displayPortVTable_s; typedef struct displayPort_s { const struct displayPortVTable_s *vTable; @@ -68,6 +73,7 @@ typedef struct displayPort_s { int8_t cursorRow; int8_t grabCount; textAttributes_t cachedSupportedTextAttributes; + uint16_t maxChar; } displayPort_t; typedef struct displayPortVTable_s { @@ -77,13 +83,14 @@ typedef struct displayPortVTable_s { int (*drawScreen)(displayPort_t *displayPort); int (*screenSize)(const displayPort_t *displayPort); int (*writeString)(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *text, textAttributes_t attr); - int (*writeChar)(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c, textAttributes_t attr); - bool (*readChar)(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t *c, textAttributes_t *attr); + int (*writeChar)(displayPort_t *displayPort, uint8_t x, uint8_t y, uint16_t c, textAttributes_t attr); + bool (*readChar)(displayPort_t *displayPort, uint8_t x, uint8_t y, uint16_t *c, textAttributes_t *attr); bool (*isTransferInProgress)(const displayPort_t *displayPort); int (*heartbeat)(displayPort_t *displayPort); void (*resync)(displayPort_t *displayPort); uint32_t (*txBytesFree)(const displayPort_t *displayPort); textAttributes_t (*supportedTextAttributes)(const displayPort_t *displayPort); + bool (*getFontMetadata)(displayFontMetadata_t *metadata, const displayPort_t *displayPort); } displayPortVTable_t; typedef struct displayPortProfile_s { @@ -104,11 +111,12 @@ int displayScreenSize(const displayPort_t *instance); void displaySetXY(displayPort_t *instance, uint8_t x, uint8_t y); int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, const char *s); int displayWriteWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, const char *s, textAttributes_t attr); -int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t c); -int displayWriteCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t c, textAttributes_t attr); -bool displayReadCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t *c, textAttributes_t *attr); +int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c); +int displayWriteCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c, textAttributes_t attr); +bool displayReadCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t *c, textAttributes_t *attr); bool displayIsTransferInProgress(const displayPort_t *instance); void displayHeartbeat(displayPort_t *instance); void displayResync(displayPort_t *instance); uint16_t displayTxBytesFree(const displayPort_t *instance); +bool displayGetFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *instance); void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable); diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 8fcd8386db..af1213845d 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -108,12 +108,18 @@ #define MAX7456_SIGNAL_CHECK_INTERVAL_MS 1000 // msec // DMM special bits +#define DMM_8BIT_MODE (1 << 6) #define DMM_BLINK (1 << 4) #define DMM_INVERT_PIXEL_COLOR (1 << 3) #define DMM_CLEAR_DISPLAY (1 << 2) #define DMM_CLEAR_DISPLAY_VERT (DMM_CLEAR_DISPLAY | 1 << 1) #define DMM_AUTOINCREMENT (1 << 0) +#define DMM_IS_8BIT_MODE(val) (val & DMM_8BIT_MODE) +#define DMM_CHAR_MODE_MASK (MAX7456_MODE_INVERT | MAX7456_MODE_BLINK | MAX7456_MODE_SOLID_BG) + +#define DMAH_8_BIT_DMDI_IS_CHAR_ATTR (1 << 1) + // Special address for terminating incremental write #define END_STRING 0xff @@ -149,17 +155,29 @@ #define MAX7456ADD_RB15 0x1f #define MAX7456ADD_OSDBL 0x6c #define MAX7456ADD_STAT 0xA0 +#define MAX7456ADD_CMDO 0xC0 #define NVM_RAM_SIZE 54 +#define READ_NVR 0x50 #define WRITE_NVR 0xA0 +// Maximum time to wait for video sync. After this we +// default to PAL +#define MAX_SYNC_WAIT_MS 1500 + +// Maximum time to wait for a software reset to complete +// Under normal conditions, it should take 20us +#define MAX_RESET_TIMEOUT_MS 50 + #define CHARS_PER_LINE 30 // XXX Should be related to VIDEO_BUFFER_CHARS_*? -#define MAKE_CHAR_MODE(c, m) ((((uint16_t)c) << 8) | m) +#define MAKE_CHAR_MODE_U8(c, m) ((((uint16_t)c) << 8) | m) +#define MAKE_CHAR_MODE(c, m) (MAKE_CHAR_MODE_U8(c, m) | (c > 255 ? CHAR_MODE_EXT : 0)) #define CHAR_BLANK MAKE_CHAR_MODE(0x20, 0) #define CHAR_BYTE(x) (x >> 8) #define MODE_BYTE(x) (x & 0xFF) - -uint16_t maxScreenSize = VIDEO_BUFFER_CHARS_PAL; +#define CHAR_IS_BLANK(x) ((CHAR_BYTE(x) == 0x20 || CHAR_BYTE(x) == 0x00) && !CHAR_MODE_IS_EXT(MODE_BYTE(x))) +#define CHAR_MODE_EXT (1 << 2) +#define CHAR_MODE_IS_EXT(m) ((m) & CHAR_MODE_EXT) // we write everything in screenBuffer and set a dirty bit // in screenIsDirty to upgrade only changed chars this solution @@ -169,14 +187,94 @@ static BITARRAY_DECLARE(screenIsDirty, VIDEO_BUFFER_CHARS_PAL); //max chars to update in one idle #define MAX_CHARS2UPDATE 10 -#define BYTES_PER_CHAR2UPDATE 8 // [3-4] spi regs + values for them +#define BYTES_PER_CHAR2UPDATE (7 * 2) // SPI regs + values for them -static bool firstInit = true; -static uint8_t videoSignalCfg = 0; -static uint8_t videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE; //PAL by default -static bool max7456Lock = false; -static bool fontIsLoading = false; -static busDevice_t * max7456dev = NULL; +typedef struct max7456Registers_s { + uint8_t vm0; + uint8_t dmm; +} max7456Registers_t; + +typedef struct max7456State_s { + busDevice_t *dev; + videoSystem_e videoSystem; + bool isInitialized; + bool mutex; + max7456Registers_t registers; +} max7456State_t; + +static max7456State_t state; + +static bool max7456ReadVM0(uint8_t *vm0) +{ + return busRead(state.dev, MAX7456ADD_VM0 | MAX7456ADD_READ, vm0); +} + +static bool max7456IsBusy(void) +{ + uint8_t status; + + if (busRead(state.dev, MAX7456ADD_STAT, &status)) { + return status & STAT_NVR_BUSY; + } + // Read failed or busy + return true; +} + +// Wait until max7456IsBusy() returns false, toggling a LED on each iteration +static void max7456WaitUntilNoBusy(void) +{ + while (1) { + if (!max7456IsBusy()) { + break; + } +#ifdef LED0_TOGGLE + LED0_TOGGLE; +#else + LED1_TOGGLE; +#endif + } +} + +// Sets wether the OSD drawing is enabled. Returns true iff +// changes were succesfully performed. +static bool max7456OSDSetEnabled(bool enabled) +{ + if (enabled && !(state.registers.vm0 | OSD_ENABLE)) { + state.registers.vm0 |= OSD_ENABLE; + } else if (!enabled && (state.registers.vm0 | OSD_ENABLE)) { + state.registers.vm0 &= ~OSD_ENABLE; + } else { + // No changes needed + return false; + } + return busWrite(state.dev, MAX7456ADD_VM0, state.registers.vm0); +} + +static bool max7456OSDIsEnabled(void) +{ + return state.registers.vm0 & OSD_ENABLE; +} + +static void max7456Lock(void) +{ + while(state.mutex); + + state.mutex = true; +} + +static void max7456Unlock(void) +{ + state.mutex = false; +} + +static bool max7456TryLock(void) +{ + if (!state.mutex) { + state.mutex = true; + return true; + } + return false; +} static int max7456PrepareBuffer(uint8_t * buf, int bufPtr, uint8_t add, uint8_t data) { @@ -185,13 +283,26 @@ static int max7456PrepareBuffer(uint8_t * buf, int bufPtr, uint8_t add, uint8_t return bufPtr; } +uint16_t max7456GetScreenSize(void) +{ + // Default to PAL while the display is not yet initialized. This + // was the initial behavior and not all callers might be able to + // deal with a zero returned from here. + // TODO: Inspect all callers, make sure they can handle zero and + // change this function to return zero before initialization. + if (state.isInitialized && (state.registers.vm0 & VIDEO_LINES_NTSC)) { + return VIDEO_BUFFER_CHARS_NTSC; + } + return VIDEO_BUFFER_CHARS_PAL; +} + uint8_t max7456GetRowsCount(void) { - if (firstInit) { + if (!state.isInitialized) { // Not initialized yet return 0; } - if (videoSignalReg & VIDEO_MODE_PAL) { + if (state.registers.vm0 & VIDEO_MODE_PAL) { return VIDEO_LINES_PAL; } @@ -201,88 +312,99 @@ uint8_t max7456GetRowsCount(void) //because MAX7456 need some time to detect video system etc. we need to wait for a while to initialize it at startup //and in case of restart we need to reinitialize chip. Note that we can't touch screenBuffer here, since //it might already have some data by the first time this function is called. -void max7456ReInit(void) +static void max7456ReInit(void) { - uint8_t buf[(VIDEO_BUFFER_CHARS_PAL + 3) * 2]; + uint8_t buf[2 * 2]; int bufPtr; - uint8_t maxScreenRows; - uint8_t srdata = 0; + uint8_t statVal; + // Check if device is available - if (max7456dev == NULL) { + if (state.dev == NULL) { return; } - //do not init MAX before camera power up correctly - if (millis() < 1500) - return; + uint8_t vm0Mode; - switch (videoSignalCfg) { + switch (state.videoSystem) { case PAL: - videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE; + vm0Mode = VIDEO_MODE_PAL; break; case NTSC: - videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE; + vm0Mode = VIDEO_MODE_NTSC; break; default: - busRead(max7456dev, MAX7456ADD_STAT, &srdata); - if ((0x02 & srdata) == 0x02) - videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE; + busRead(state.dev, MAX7456ADD_STAT, &statVal); + if (VIN_IS_PAL(statVal) || millis() > MAX_SYNC_WAIT_MS) { + vm0Mode = VIDEO_MODE_PAL; + } else if (VIN_IS_NTSC_alt(statVal)) { + vm0Mode = VIDEO_MODE_NTSC; + } else { + // No signal detected yet, wait for detection timeout + return; + } } - if (videoSignalReg & VIDEO_MODE_PAL) { //PAL - maxScreenSize = VIDEO_BUFFER_CHARS_PAL; - maxScreenRows = VIDEO_LINES_PAL; - } else { // NTSC - maxScreenSize = VIDEO_BUFFER_CHARS_NTSC; - maxScreenRows = VIDEO_LINES_NTSC; - } + state.registers.vm0 = vm0Mode | OSD_ENABLE; - // set all rows to same charactor black/white level - bufPtr = 0; - for (int x = 0; x < maxScreenRows; x++) { - bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_RB0 + x, BWBRIGHTNESS); - } - - // make sure the Max7456 is enabled - bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_VM0, videoSignalReg); - bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_VM1, BLINK_DUTY_CYCLE_50_50 | BLINK_TIME_3 | BACKGROUND_BRIGHTNESS_28); + // Enable OSD drawing and clear the display + bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_VM0, state.registers.vm0); bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_DMM, DMM_CLEAR_DISPLAY); // Transfer data to SPI - busTransfer(max7456dev, NULL, buf, bufPtr); + busTransfer(state.dev, NULL, buf, bufPtr); - // force redrawing all screen in non-dma mode - memset(screenIsDirty, 0xFF, sizeof(screenIsDirty)); - if (firstInit) { + // force redrawing all screen + BITARRAY_SET_ALL(screenIsDirty); + if (!state.isInitialized) { max7456RefreshAll(); - firstInit = false; + state.isInitialized = true; } } - //here we init only CS and try to init MAX for first time void max7456Init(const videoSystem_e videoSystem) { - max7456dev = busDeviceInit(BUSTYPE_SPI, DEVHW_MAX7456, 0, OWNER_OSD); + uint8_t buf[(VIDEO_LINES_PAL + 1) * 2]; + int bufPtr; + state.dev = busDeviceInit(BUSTYPE_SPI, DEVHW_MAX7456, 0, OWNER_OSD); - if (max7456dev == NULL) { + if (state.dev == NULL) { return; } - busSetSpeed(max7456dev, BUS_SPEED_STANDARD); + busSetSpeed(state.dev, BUS_SPEED_STANDARD); // force soft reset on Max7456 - busWrite(max7456dev, MAX7456ADD_VM0, MAX7456_RESET); + busWrite(state.dev, MAX7456ADD_VM0, MAX7456_RESET); - videoSignalCfg = videoSystem; + // DMM defaults to all zeroes on reset + state.registers.dmm = 0; + state.videoSystem = videoSystem; // Set screenbuffer to all blanks for (uint_fast16_t ii = 0; ii < ARRAYLEN(screenBuffer); ii++) { screenBuffer[ii] = CHAR_BLANK; } - //real init will be made letter when driver idle detect + // Wait for software reset to finish + timeMs_t timeout = millis() + MAX_RESET_TIMEOUT_MS; + while(max7456ReadVM0(&state.registers.vm0) && + (state.registers.vm0 | MAX7456_RESET) && + millis() < timeout); + + // Set all rows to same charactor black/white level. We + // can do this without finding wether the display is PAL + // NTSC because all the brightness registers can be written + // regardless of the video mode. + bufPtr = 0; + for (int ii = 0; ii < VIDEO_LINES_PAL; ii++) { + bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_RB0 + ii, BWBRIGHTNESS); + } + + // Set the blink duty cycle + bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_VM1, BLINK_DUTY_CYCLE_50_50 | BLINK_TIME_3 | BACKGROUND_BRIGHTNESS_28); + busTransfer(state.dev, NULL, buf, bufPtr); } void max7456ClearScreen(void) @@ -295,7 +417,7 @@ void max7456ClearScreen(void) } } -void max7456WriteChar(uint8_t x, uint8_t y, uint8_t c, uint8_t mode) +void max7456WriteChar(uint8_t x, uint8_t y, uint16_t c, uint8_t mode) { unsigned pos = y * CHARS_PER_LINE + x; uint16_t val = MAKE_CHAR_MODE(c, mode); @@ -305,13 +427,17 @@ void max7456WriteChar(uint8_t x, uint8_t y, uint8_t c, uint8_t mode) } } -bool max7456ReadChar(uint8_t x, uint8_t y, uint8_t *c, uint8_t *mode) +bool max7456ReadChar(uint8_t x, uint8_t y, uint16_t *c, uint8_t *mode) { unsigned pos = y * CHARS_PER_LINE + x; if (pos < ARRAYLEN(screenBuffer)) { uint16_t val = screenBuffer[pos]; - *c = val >> 8; - *mode = val & 0xFF; + *c = CHAR_BYTE(val); + *mode = MODE_BYTE(val); + if (CHAR_MODE_IS_EXT(*mode)) { + *c |= 1 << 8; + *mode &= ~CHAR_MODE_EXT; + } return true; } return false; @@ -327,7 +453,7 @@ void max7456Write(uint8_t x, uint8_t y, const char *buff, uint8_t mode) if (x + i >= CHARS_PER_LINE) { break; } - c = MAKE_CHAR_MODE(*buff, mode); + c = MAKE_CHAR_MODE_U8(*buff, mode); if (screenBuffer[pos] != c) { screenBuffer[pos] = c; bitArraySet(screenIsDirty, pos); @@ -335,102 +461,134 @@ void max7456Write(uint8_t x, uint8_t y, const char *buff, uint8_t mode) } } -void max7456DrawScreenPartial(void) +// Must be called with the lock held. Returns wether any new characters +// were drawn. +static bool max7456DrawScreenPartial(void) +{ + uint8_t spiBuff[MAX_CHARS2UPDATE * BYTES_PER_CHAR2UPDATE]; + int bufPtr = 0; + int pos; + uint_fast16_t updatedCharCount; + uint8_t charMode; + + for (pos = 0, updatedCharCount = 0;;) { + pos = BITARRAY_FIND_FIRST_SET(screenIsDirty, pos); + if (pos < 0) { + // No more dirty chars. + break; + } + + // Found one dirty character to send + uint8_t ph = pos >> 8; + uint8_t pl = pos & 0xff; + + charMode = MODE_BYTE(screenBuffer[pos]); + uint8_t chr = CHAR_BYTE(screenBuffer[pos]); + if (CHAR_MODE_IS_EXT(charMode)) { + if (!DMM_IS_8BIT_MODE(state.registers.dmm)) { + state.registers.dmm |= DMM_8BIT_MODE; + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMM, state.registers.dmm); + } + + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, ph | DMAH_8_BIT_DMDI_IS_CHAR_ATTR); + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, pl); + // Attribute bit positions on DMDI are 2 bits up relative to DMM. + // DMM uses [5:3] while DMDI uses [7:4] - one bit more for referencing + // characters in the [256, 511] range (which is not possible via DMM). + // Since we write mostly to DMM, the internal representation uses + // the format of the former and we shift it up here. + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, charMode << 2); + + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, ph); + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, pl); + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, chr); + + } else { + if (DMM_IS_8BIT_MODE(state.registers.dmm) || (DMM_CHAR_MODE_MASK & state.registers.dmm) != charMode) { + state.registers.dmm &= ~DMM_8BIT_MODE; + state.registers.dmm = (state.registers.dmm & ~DMM_CHAR_MODE_MASK) | charMode; + // Send the attributes for the character run. They + // will be applied to all characters until we change + // the DMM register. + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMM, state.registers.dmm); + } + + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, ph); + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, pl); + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, chr); + } + + bitArrayClr(screenIsDirty, pos); + if (++updatedCharCount == MAX_CHARS2UPDATE) { + break; + } + // Start next search at next bit + pos++; + } + + if (bufPtr) { + busTransfer(state.dev, NULL, spiBuff, bufPtr); + return true; + } + return false; +} + +// Must be called with the lock held +static void max7456StallCheck(void) { static uint32_t lastSigCheckMs = 0; static uint32_t videoDetectTimeMs = 0; - // Save this between updates. The default value - // in the MAX7456 is all bits to zero. - static uint8_t setMode = 0; - uint8_t stallCheck; - uint8_t videoSense; - uint32_t nowMs; - int pos; - uint_fast16_t updatedCharCount; - uint8_t currentMode; + uint8_t vm0; + uint8_t statReg; - // Check if device is available - if (max7456dev == NULL) { + if (!state.isInitialized || !max7456ReadVM0(&vm0) || vm0 != state.registers.vm0) { + max7456ReInit(); return; } - if (!max7456Lock && !fontIsLoading) { - // (Re)Initialize MAX7456 at startup or stall is detected. - - max7456Lock = true; - - // Stall check - busRead(max7456dev, MAX7456ADD_VM0 | MAX7456ADD_READ, &stallCheck); - - nowMs = millis(); - - if (stallCheck != videoSignalReg) { - max7456ReInit(); - } else if ((videoSignalCfg == VIDEO_SYSTEM_AUTO) - && ((nowMs - lastSigCheckMs) > MAX7456_SIGNAL_CHECK_INTERVAL_MS)) { + if (state.videoSystem == VIDEO_SYSTEM_AUTO) { + timeMs_t nowMs = millis(); + if ((nowMs - lastSigCheckMs) > MAX7456_SIGNAL_CHECK_INTERVAL_MS) { // Adjust output format based on the current input format. - busRead(max7456dev, MAX7456ADD_STAT, &videoSense); + busRead(state.dev, MAX7456ADD_STAT, &statReg); - if (videoSense & STAT_LOS) { + if (statReg & STAT_LOS) { videoDetectTimeMs = 0; } else { - if ((VIN_IS_PAL(videoSense) && VIDEO_MODE_IS_NTSC(videoSignalReg)) - || (VIN_IS_NTSC_alt(videoSense) && VIDEO_MODE_IS_PAL(videoSignalReg))) { + if ((VIN_IS_PAL(statReg) && VIDEO_MODE_IS_NTSC(state.registers.vm0)) + || (VIN_IS_NTSC_alt(statReg) && VIDEO_MODE_IS_PAL(state.registers.vm0))) { if (videoDetectTimeMs) { - if (millis() - videoDetectTimeMs > VIDEO_SIGNAL_DEBOUNCE_MS) { + if (nowMs - videoDetectTimeMs > VIDEO_SIGNAL_DEBOUNCE_MS) { max7456ReInit(); } } else { // Wait for signal to stabilize - videoDetectTimeMs = millis(); + videoDetectTimeMs = nowMs; } } } lastSigCheckMs = nowMs; } + } +} - //------------ end of (re)init------------------------------------- - uint8_t spiBuff[MAX_CHARS2UPDATE * BYTES_PER_CHAR2UPDATE]; - int bufPtr = 0; +void max7456Update(void) +{ + // Check if device is available + if (state.dev == NULL) { + return; + } - for (pos = 0, updatedCharCount = 0;;) { - pos = BITARRAY_FIND_FIRST_SET(screenIsDirty, pos); - if (pos < 0 || pos >= maxScreenSize) { - // No more dirty chars. - break; - } - - currentMode = MODE_BYTE(screenBuffer[pos]); - - // Found one dirty character to send - if (setMode != currentMode) { - setMode = currentMode; - // Send the attributes for the character run. They - // will be applied to all characters until we change - // the DMM register. - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMM, currentMode); - } - - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, pos >> 8); - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, pos & 0xff); - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, CHAR_BYTE(screenBuffer[pos])); - - bitArrayClr(screenIsDirty, pos); - if (++updatedCharCount == MAX_CHARS2UPDATE) { - break; - } - // Start next search at next bit - pos++; + if ((max7456OSDIsEnabled() && max7456TryLock()) || !state.isInitialized) { + // (Re)Initialize MAX7456 at startup or stall is detected. + max7456StallCheck(); + if (state.isInitialized) { + max7456DrawScreenPartial(); } - - if (bufPtr) { - busTransfer(max7456dev, NULL, spiBuff, bufPtr); - } - - max7456Lock = false; + max7456Unlock(); } } @@ -439,115 +597,126 @@ void max7456DrawScreenPartial(void) // when copter is armed. void max7456RefreshAll(void) { - uint8_t spiBuff[(VIDEO_BUFFER_CHARS_PAL + 4) * 2]; - int bufPtr; + uint8_t dmm; // Check if device is available - if (max7456dev == NULL) { + if (state.dev == NULL) { return; } - if (!max7456Lock) { - uint16_t xx; - max7456Lock = true; + if (max7456TryLock()) { - // Write characters. Start at character zero. - bufPtr = 0; - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, 0); - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, 0); - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMM, DMM_AUTOINCREMENT); + // Issue an OSD clear command + busRead(state.dev, MAX7456ADD_DMM | MAX7456ADD_READ, &dmm); + busWrite(state.dev, MAX7456ADD_DMM, state.registers.dmm | DMM_CLEAR_DISPLAY); - for (xx = 0; xx < maxScreenSize; ++xx) { - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, CHAR_BYTE(screenBuffer[xx])); + // Wait for clear to complete (20us) + while (1) { + busRead(state.dev, MAX7456ADD_DMM | MAX7456ADD_READ, &dmm); + if (!(dmm & DMM_CLEAR_DISPLAY)) { + state.registers.dmm = dmm; + break; + } } - // Exit auto-increment mode by writing the 0xFF escape sequence to DMDI. - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, 0xFF); - - // Write chunk of data - busTransfer(max7456dev, NULL, spiBuff, bufPtr); - - // Write character attributes. Start at zero, but - // set DMAH[1] = 1, to signal that we're sending - // attributes rather than characters. Process is the - // same as for the characters a few lines up. - bufPtr = 0; - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, 1<<1); - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, 0); - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMM, DMM_AUTOINCREMENT); - - for (xx = 0; xx < maxScreenSize; ++xx) { - // Note that atttribute bits in DMDI are in different - // positions than in DMM (DMM is used for partial writes), - // and we store the attributes in the format expected by - // DMM. - // | LBC | BLK | INV - // ---------------------- - // DMDI:| 7 | 6 | 5 - // DMM: | 5 | 4 | 3 - // - // Thus, we need to shift the bits by 2 when writing character - // attributes to DMDI. - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, MODE_BYTE(screenBuffer[xx]) << 2); + // Mark non-blank characters as dirty + BITARRAY_CLR_ALL(screenIsDirty); + for (unsigned ii = 0; ii < ARRAYLEN(screenBuffer); ii++) { + if (!CHAR_IS_BLANK(screenBuffer[ii])) { + bitArraySet(screenIsDirty, ii); + } } - // Exit auto-increment mode by writing the 0xFF escape sequence to DMDI. - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, 0xFF); + // Now perform partial writes until there are no dirty ones + while(max7456DrawScreenPartial()); - // Write chunk of data - busTransfer(max7456dev, NULL, spiBuff, bufPtr); - - // All characters have been set to the MAX7456, none is dirty now. - memset(screenIsDirty, 0, sizeof(screenIsDirty)); - max7456Lock = false; + max7456Unlock(); } } -void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data) +void max7456ReadNvm(uint16_t char_address, max7456Character_t *chr) { - uint8_t spiBuff[(54 * 2 + 3) * 2]; - int bufPtr = 0; - // Check if device is available - if (max7456dev == NULL) { + if (state.dev == NULL) { return; } - while (max7456Lock); - max7456Lock = true; + max7456Lock(); + // OSD must be disabled to read or write to NVM + bool enabled = max7456OSDSetEnabled(false); - // disable display - fontIsLoading = true; + busWrite(state.dev, MAX7456ADD_CMAH, char_address); + if (char_address > 255) { + // AT7456E and AB7456 have 512 characters of NVM. + // To read/write to NVM they use CMAL[6] as the + // high bits of the character address. + uint8_t addr_h = char_address >> 8; + busWrite(state.dev, MAX7456ADD_CMAL, addr_h << 6); + } + busWrite(state.dev, MAX7456ADD_CMM, READ_NVR); - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_VM0, 0); - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMAH, char_address); // set start address high + max7456WaitUntilNoBusy(); - for (int x = 0; x < 54; x++) { - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMAL, x); //set start address low - bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMDI, font_data[x]); + for (unsigned ii = 0; ii < sizeof(chr->data); ii++) { + busWrite(state.dev, MAX7456ADD_CMAL, ii); + busRead(state.dev, MAX7456ADD_CMDO, &chr->data[ii]); + } + + max7456OSDSetEnabled(enabled); + max7456Unlock(); +} + +void max7456WriteNvm(uint16_t char_address, const max7456Character_t *chr) +{ + uint8_t spiBuff[(sizeof(chr->data) * 2 + 2) * 2]; + int bufPtr = 0; + + // Check if device is available + if (state.dev == NULL) { + return; + } + + max7456Lock(); + // OSD must be disabled to read or write to NVM + max7456OSDSetEnabled(false); + + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMAH, char_address & 0xFF); // set start address high + + uint8_t or_val = 0; + if (char_address > 255) { + // AT7456E and AB7456 have 512 characters of NVM. + // To read/write to NVM they use CMAL[6] as the + // high bit of the character address. + // + // Instead of issuing an additional write to CMAL when + // we're done uploading to shadow RAM, we set the high + // bits of CMAL on every write since they have no side + // effects while writing from CMDI to RAM and when we + // issue the copy command to NVM, CMAL[6] is already + // set. + uint8_t addr_h = char_address >> 8; + or_val = addr_h << 6; + } + + for (unsigned x = 0; x < sizeof(chr->data); x++) { + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMAL, x | or_val); //set start address low + bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMDI, chr->data[x]); } // transfer 54 bytes from shadow ram to NVM bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMM, WRITE_NVR); - busTransfer(max7456dev, NULL, spiBuff, bufPtr); + busTransfer(state.dev, NULL, spiBuff, bufPtr); - while (1) { - uint8_t status; + max7456WaitUntilNoBusy(); - busRead(max7456dev, MAX7456ADD_STAT, &status); - if ((status & STAT_NVR_BUSY) == 0x00) { - break; - } + /* XXX: Don't call max7456OSDEnable(), it's intentionally ommited. + * If we continue drawing while characters are being uploaded, we + * get some corrupted characters from time to time. As a workaround, + * we require a reboot after characters have been uploaded to NVM. + */ -#ifdef LED0_TOGGLE - LED0_TOGGLE; -#else - LED1_TOGGLE; -#endif - } - - max7456Lock = false; + max7456Unlock(); } diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h index 2227ec7483..be894adde1 100644 --- a/src/main/drivers/max7456.h +++ b/src/main/drivers/max7456.h @@ -41,16 +41,19 @@ enum VIDEO_TYPES { AUTO = 0, PAL, NTSC }; #define MAX7456_MODE_BLINK (1 << 4) #define MAX7456_MODE_SOLID_BG (1 << 5) -extern uint16_t maxScreenSize; +typedef struct max7456Character_s { + uint8_t data[54]; +} max7456Character_t; -struct vcdProfile_s; -void max7456Init(const videoSystem_e videoSystem); -void max7456DrawScreenPartial(void); -void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data); +void max7456Init(const videoSystem_e videoSystem); +void max7456Update(void); +void max7456ReadNvm(uint16_t char_address, max7456Character_t *chr); +void max7456WriteNvm(uint16_t char_address, const max7456Character_t *chr); +uint16_t max7456GetScreenSize(void); uint8_t max7456GetRowsCount(void); -void max7456Write(uint8_t x, uint8_t y, const char *buff, uint8_t mode); -void max7456WriteChar(uint8_t x, uint8_t y, uint8_t c, uint8_t mode); -bool max7456ReadChar(uint8_t x, uint8_t y, uint8_t *c, uint8_t *mode); -void max7456ClearScreen(void); -void max7456RefreshAll(void); +void max7456Write(uint8_t x, uint8_t y, const char *buff, uint8_t mode); +void max7456WriteChar(uint8_t x, uint8_t y, uint16_t c, uint8_t mode); +bool max7456ReadChar(uint8_t x, uint8_t y, uint16_t *c, uint8_t *mode); +void max7456ClearScreen(void); +void max7456RefreshAll(void); uint8_t* max7456GetScreenBuffer(void); diff --git a/src/main/drivers/max7456_symbols.h b/src/main/drivers/max7456_symbols.h index 477d9973db..7cb39c64d5 100644 --- a/src/main/drivers/max7456_symbols.h +++ b/src/main/drivers/max7456_symbols.h @@ -196,6 +196,10 @@ #define SYM_AH_V_START 0xE0 // 224 to 229 Vertical AHI +#define SYM_LOGO_START 0x101 // 257 to 280, INAV logo +#define SYM_LOGO_WIDTH 6 +#define SYM_LOGO_HEIGHT 4 + #define SYM_CURSOR SYM_AH_LEFT // Menu cursor #endif // USE_MAX7456 diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index ad3bff71dc..71e3325da8 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2140,13 +2140,18 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP_OSD_CHAR_WRITE: #ifdef USE_MAX7456 if (dataSize >= 55) { - uint8_t font_data[64]; - const uint8_t addr = sbufReadU8(src); - for (int i = 0; i < 54; i++) { - font_data[i] = sbufReadU8(src); + max7456Character_t chr; + uint16_t addr; + if (dataSize >= 56) { + addr = sbufReadU16(src); + } else { + addr = sbufReadU8(src); + } + for (unsigned ii = 0; ii < sizeof(chr.data); ii++) { + chr.data[ii] = sbufReadU8(src); } // !!TODO - replace this with a device independent implementation - max7456WriteNvm(addr, font_data); + max7456WriteNvm(addr, &chr); } else return MSP_RESULT_ERROR; #endif // USE_MAX7456 diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c index d086b36f32..cba52cbac1 100644 --- a/src/main/io/displayport_max7456.c +++ b/src/main/io/displayport_max7456.c @@ -33,6 +33,13 @@ #include "io/displayport_max7456.h" +// 'I', 'N', 'A', 'V', 1 +#define CHR_IS_METADATA(chr) ((chr)->data[0] == 'I' && \ + (chr)->data[1] == 'N' && \ + (chr)->data[2] == 'A' && \ + (chr)->data[3] == 'V' && \ + (chr)->data[4] == 1) + displayPort_t max7456DisplayPort; static uint8_t max7456Mode(textAttributes_t attr) @@ -72,7 +79,7 @@ static int clearScreen(displayPort_t *displayPort) static int drawScreen(displayPort_t *displayPort) { UNUSED(displayPort); - max7456DrawScreenPartial(); + max7456Update(); return 0; } @@ -80,7 +87,7 @@ static int drawScreen(displayPort_t *displayPort) static int screenSize(const displayPort_t *displayPort) { UNUSED(displayPort); - return maxScreenSize; + return max7456GetScreenSize(); } static int writeString(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s, textAttributes_t attr) @@ -91,7 +98,7 @@ static int writeString(displayPort_t *displayPort, uint8_t x, uint8_t y, const c return 0; } -static int writeChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c, textAttributes_t attr) +static int writeChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint16_t c, textAttributes_t attr) { UNUSED(displayPort); max7456WriteChar(x, y, c, max7456Mode(attr)); @@ -99,7 +106,7 @@ static int writeChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c return 0; } -static bool readChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t *c, textAttributes_t *attr) +static bool readChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint16_t *c, textAttributes_t *attr) { UNUSED(displayPort); return max7456ReadChar(x, y, c, attr); @@ -142,6 +149,31 @@ static textAttributes_t supportedTextAttributes(const displayPort_t *displayPort return attr; } +static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *displayPort) +{ + UNUSED(displayPort); + + max7456Character_t chr; + + max7456ReadNvm(255, &chr); + + if (CHR_IS_METADATA(&chr)) { + metadata->version = chr.data[5]; + // Not all MAX7456 chips support 512 characters. To detect this, + // we place metadata in both characters 255 and 256. This way we + // can find out how many characters the font in NVM has. + max7456ReadNvm(256, &chr); + if (CHR_IS_METADATA(&chr)) { + metadata->charCount = 512; + } else { + metadata->charCount = 256; + } + return true; + } + + return false; +} + static const displayPortVTable_t max7456VTable = { .grab = grab, .release = release, @@ -156,12 +188,13 @@ static const displayPortVTable_t max7456VTable = { .resync = resync, .txBytesFree = txBytesFree, .supportedTextAttributes = supportedTextAttributes, + .getFontMetadata = getFontMetadata, }; displayPort_t *max7456DisplayPortInit(const videoSystem_e videoSystem) { - displayInit(&max7456DisplayPort, &max7456VTable); max7456Init(videoSystem); + displayInit(&max7456DisplayPort, &max7456VTable); resync(&max7456DisplayPort); return &max7456DisplayPort; } diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c index f12969c81b..27fc439e0a 100644 --- a/src/main/io/displayport_msp.c +++ b/src/main/io/displayport_msp.c @@ -113,7 +113,7 @@ static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, con return output(displayPort, MSP_DISPLAYPORT, buf, len + 4); } -static int writeChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint8_t c, textAttributes_t attr) +static int writeChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t c, textAttributes_t attr) { char buf[2]; diff --git a/src/main/io/displayport_oled.c b/src/main/io/displayport_oled.c index 51989bdcfb..bc132e524b 100644 --- a/src/main/io/displayport_oled.c +++ b/src/main/io/displayport_oled.c @@ -66,7 +66,7 @@ static int oledWriteString(displayPort_t *displayPort, uint8_t x, uint8_t y, con return 0; } -static int oledWriteChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c, textAttributes_t attr) +static int oledWriteChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint16_t c, textAttributes_t attr) { UNUSED(displayPort); UNUSED(attr); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index d8e7e744ff..fd79d1a0ca 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -124,8 +124,16 @@ x; \ }) +#define OSD_CHR_IS_NUM(c) (c >= '0' && c <= '9') + +#define OSD_CENTER_LEN(x) ((osdDisplayPort->cols - x) / 2) +#define OSD_CENTER_S(s) OSD_CENTER_LEN(strlen(s)) + +#define OSD_MIN_FONT_VERSION 1 + static unsigned currentLayout = 0; static int layoutOverride = -1; +static bool hasExtendedFont = false; // Wether the font supports characters > 256 typedef struct statistic_s { uint16_t max_speed; @@ -1021,7 +1029,7 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente } } else { - uint8_t c; + uint16_t c; if (displayReadCharWithAttr(osdDisplayPort, poiX, poiY, &c, NULL) && c != SYM_BLANK) { // Something else written here, increase scale. If the display doesn't support reading // back characters, we assume there's nothing. @@ -1673,7 +1681,7 @@ static bool osdDrawSingleElement(uint8_t item) float fy = dx * (ky / kx) + pitchAngle * pitch_rad_to_char + 0.49f; int8_t dy = floorf(fy); const uint8_t chX = elemPosX + dx, chY = elemPosY - dy; - uint8_t c; + uint16_t c; if ((dy >= -AH_HEIGHT / 2) && (dy <= AH_HEIGHT / 2) && displayReadCharWithAttr(osdDisplayPort, chX, chY, &c, NULL) && (c == SYM_BLANK)) { c = SYM_AH_H_START + ((AH_H_SYM_COUNT - 1) - (uint8_t)((fy - dy) * AH_H_SYM_COUNT)); @@ -1690,7 +1698,7 @@ static bool osdDrawSingleElement(uint8_t item) const float fx = (dy - pitchAngle * pitch_rad_to_char) * (kx / ky) + 0.5f; const int8_t dx = floorf(fx); const uint8_t chX = elemPosX + dx, chY = elemPosY - dy; - uint8_t c; + uint16_t c; if ((dx >= -AH_WIDTH / 2) && (dx <= AH_WIDTH / 2) && displayReadCharWithAttr(osdDisplayPort, chX, chY, &c, NULL) && (c == SYM_BLANK)) { c = SYM_AH_V_START + (fx - dx) * AH_V_SYM_COUNT; @@ -2545,7 +2553,29 @@ void osdInit(displayPort_t *osdDisplayPortToUse) displayClearScreen(osdDisplayPort); - uint8_t y = 4; + uint8_t y = 1; + displayFontMetadata_t metadata; + bool fontHasMetadata = displayGetFontMetadata(&metadata, osdDisplayPort); + + if (fontHasMetadata && metadata.charCount > 256) { + hasExtendedFont = true; + unsigned logo_c = SYM_LOGO_START; + unsigned logo_x = OSD_CENTER_LEN(SYM_LOGO_WIDTH); + for (unsigned ii = 0; ii < SYM_LOGO_HEIGHT; ii++) { + for (unsigned jj = 0; jj < SYM_LOGO_WIDTH; jj++) { + displayWriteChar(osdDisplayPort, logo_x + jj, y, logo_c++); + } + y++; + } + y++; + } else { + if (!fontHasMetadata || metadata.version < OSD_MIN_FONT_VERSION) { + const char *m = "INVALID FONT"; + displayWrite(osdDisplayPort, OSD_CENTER_S(m), 3, m); + } + y = 4; + } + char string_buffer[30]; tfp_sprintf(string_buffer, "INAV VERSION: %s", FC_VERSION_STRING); displayWrite(osdDisplayPort, 5, y++, string_buffer);