src/main/io/displayport_msp_osd.c

   1 /*
   2  * This file is part of INAV Project.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  */
  24
  25 #include <stdbool.h>
  26 #include <stdint.h>
  27 #include <string.h>
  28
  29 #include "platform.h"
  30
  31 #if defined(USE_OSD) && defined(USE_MSP_OSD)
  32
  33 #include "common/utils.h"
  34 #include "common/printf.h"
  35 #include "common/time.h"
  36 #include "common/bitarray.h"
  37
  38 #include "cms/cms.h"
  39
  40 #include "drivers/display.h"
  41 #include "drivers/display_font_metadata.h"
  42 #include "drivers/osd_symbols.h"
  43
  44 #include "fc/rc_modes.h"
  45 #include "fc/runtime_config.h"
  46
  47 #include "io/osd.h"
  48 #include "io/displayport_msp.h"
  49
  50 #include "msp/msp_protocol.h"
  51 #include "msp/msp_serial.h"
  52
  53 #include "displayport_msp_osd.h"
  54 #include "displayport_msp_dji_compat.h"
  55
  56 #define FONT_VERSION 3
  57
  58 typedef enum {          // defines are from hdzero code
  59     SD_3016,
  60     HD_5018,
  61     HD_3016,           // Special HDZERO mode that just sends the centre 30x16 of the 50x18 canvas to the VRX
  62     HD_6022,           // added to support DJI wtfos 60x22 grid
  63     HD_5320            // added to support Avatar and BetaflightHD
  64 } resolutionType_e;
  65
  66 #define DRAW_FREQ_DENOM 4 // 60Hz
  67 #define TX_BUFFER_SIZE 1024
  68 #define VTX_TIMEOUT 1000 // 1 second timer
  69
  70 static mspProcessCommandFnPtr mspProcessCommand;
  71 static mspPort_t mspPort;
  72 static displayPort_t mspOsdDisplayPort;
  73 static bool vtxSeen, vtxActive, vtxReset;
  74 static timeMs_t vtxHeartbeat;
  75 static timeMs_t sendSubFrameMs = 0;
  76
  77 // PAL screen size
  78 #define PAL_COLS 30
  79 #define PAL_ROWS 16
  80 // NTSC screen size
  81 #define NTSC_COLS 30
  82 #define NTSC_ROWS 13
  83 // HDZERO screen size
  84 #define HDZERO_COLS 50
  85 #define HDZERO_ROWS 18
  86 // Avatar screen size
  87 #define AVATAR_COLS 53
  88 #define AVATAR_ROWS 20
  89 // DJIWTF screen size
  90 #define DJI_COLS 60
  91 #define DJI_ROWS 22
  92
  93 // set COLS and ROWS to largest size available
  94 #define COLS DJI_COLS
  95 #define ROWS DJI_ROWS
  96
  97 // set screen size
  98 #define SCREENSIZE (ROWS*COLS)
  99
 100 static uint8_t currentOsdMode;               // HDZero screen mode can change across layouts
 101
 102 static uint8_t screen[SCREENSIZE];
 103 static uint8_t attrs[SCREENSIZE];            // font page, blink and other attributes
 104 static BITARRAY_DECLARE(dirty, SCREENSIZE);  // change status for each character on the screen
 105 static bool screenCleared;
 106 static uint8_t screenRows, screenCols;
 107 static videoSystem_e osdVideoSystem;
 108
 109 extern uint8_t cliMode;
 110
 111 static void checkVtxPresent(void)
 112 {
 113     if (vtxActive && (millis()-vtxHeartbeat) > VTX_TIMEOUT) {
 114         vtxActive = false;
 115     }
 116
 117     if (ARMING_FLAG(SIMULATOR_MODE_HITL)) {
 118         vtxActive = true;
 119     }
 120 }
 121
 122 static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *subcmd, int len)
 123 {
 124     UNUSED(displayPort);
 125
 126     checkVtxPresent();
 127
 128     int sent = 0;
 129     if (!cliMode && vtxActive) {
 130         sent = mspSerialPushPort(cmd, subcmd, len, &mspPort, MSP_V1);
 131     }
 132
 133     return sent;
 134 }
 135
 136 static uint8_t determineHDZeroOsdMode(void)
 137 {
 138     if (cmsInMenu) {
 139         return HD_5018;
 140     }
 141
 142     // Check if all visible widgets are in the center 30x16 chars of the canvas.
 143     int activeLayout = osdGetActiveLayout(NULL);
 144     osd_items_e index = 0;
 145     do {
 146         index = osdIncElementIndex(index);
 147         uint16_t pos = osdLayoutsConfig()->item_pos[activeLayout][index];
 148         if (OSD_VISIBLE(pos)) {
 149             uint8_t elemPosX = OSD_X(pos);
 150             uint8_t elemPosY = OSD_Y(pos);
 151             if (!osdItemIsFixed(index) && (elemPosX < 10 || elemPosX > 39 || elemPosY == 0 || elemPosY == 17)) {
 152                 return HD_5018;
 153             }
 154         }
 155     } while (index > 0);
 156
 157     return HD_3016;
 158 }
 159
 160
 161 uint8_t setAttrPage(uint8_t origAttr, uint8_t page)
 162 {
 163         return (origAttr & ~DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK) | (page & DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK);
 164 }
 165
 166 uint8_t setAttrBlink(uint8_t origAttr, uint8_t blink)
 167 {
 168     return (origAttr & ~DISPLAYPORT_MSP_ATTR_BLINK_MASK) | ((blink << DISPLAYPORT_MSP_ATTR_BLINK) & DISPLAYPORT_MSP_ATTR_BLINK_MASK);
 169 }
 170
 171 uint8_t setAttrVersion(uint8_t origAttr, uint8_t version)
 172 {
 173     return (origAttr & ~DISPLAYPORT_MSP_ATTR_VERSION_MASK) | ((version << DISPLAYPORT_MSP_ATTR_VERSION) & DISPLAYPORT_MSP_ATTR_VERSION_MASK);
 174 }
 175
 176 static int setDisplayMode(displayPort_t *displayPort)
 177 {
 178     if (osdVideoSystem == VIDEO_SYSTEM_HDZERO) {
 179         currentOsdMode = determineHDZeroOsdMode(); // Can change between layouts
 180     }
 181
 182     uint8_t subcmd[] = { MSP_DP_OPTIONS, 0, currentOsdMode }; // Font selection, mode (SD/HD)
 183     return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
 184 }
 185
 186 static void init(void)
 187 {
 188     memset(screen, SYM_BLANK, sizeof(screen));
 189     memset(attrs, 0, sizeof(attrs));
 190     BITARRAY_CLR_ALL(dirty);
 191 }
 192
 193 static int clearScreen(displayPort_t *displayPort)
 194 {
 195     uint8_t subcmd[] = { MSP_DP_CLEAR_SCREEN };
 196
 197     if (!cmsInMenu && IS_RC_MODE_ACTIVE(BOXOSD)) { // OSD is off
 198         output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
 199         subcmd[0] = MSP_DP_DRAW_SCREEN;
 200         vtxReset = true;
 201     }
 202     else {
 203         init();
 204         setDisplayMode(displayPort);
 205         screenCleared = true;
 206     }
 207
 208     return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
 209 }
 210
 211 static bool readChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t *c, textAttributes_t *attr)
 212 {
 213     UNUSED(displayPort);
 214
 215     uint16_t pos = (row * COLS) + col;
 216     if (pos >= SCREENSIZE) {
 217         return false;
 218     }
 219
 220     *c = screen[pos];
 221     uint8_t page = getAttrPage(attrs[pos]);
 222     *c |= page << 8;
 223
 224     if (attr) {
 225         *attr = TEXT_ATTRIBUTES_NONE;
 226     }
 227
 228     return true;
 229 }
 230
 231 static int setChar(const uint16_t pos, const uint16_t c, textAttributes_t attr)
 232 {
 233     if (pos < SCREENSIZE) {
 234         uint8_t ch = c & 0xFF;
 235         uint8_t page = (c >> 8) & DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK;
 236         if (screen[pos] != ch || getAttrPage(attrs[pos]) != page) {
 237             screen[pos] = ch;
 238             attrs[pos] = setAttrPage(attrs[pos], page);
 239             uint8_t blink = (TEXT_ATTRIBUTES_HAVE_BLINK(attr)) ? 1 : 0;
 240             attrs[pos] = setAttrBlink(attrs[pos], blink);
 241             bitArraySet(dirty, pos);
 242         }
 243     }
 244     return 0;
 245 }
 246
 247 static int writeChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t c, textAttributes_t attr)
 248 {
 249     UNUSED(displayPort);
 250
 251     return setChar((row * COLS) + col, c, attr);
 252 }
 253
 254 static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string, textAttributes_t attr)
 255 {
 256     UNUSED(displayPort);
 257
 258     uint16_t pos = (row * COLS) + col;
 259     while (*string) {
 260         setChar(pos++, *string++, attr);
 261     }
 262     return 0;
 263 }
 264
 265 /**
 266  * Write only changed characters to the VTX
 267  */
 268 static int drawScreen(displayPort_t *displayPort) // 250Hz
 269 {
 270     static uint8_t counter = 0;
 271
 272     if ((!cmsInMenu && IS_RC_MODE_ACTIVE(BOXOSD)) || (counter++ % DRAW_FREQ_DENOM)) { // 62.5Hz
 273         return 0;
 274     }
 275
 276     if (osdConfig()->msp_displayport_fullframe_interval >= 0 && (millis() > sendSubFrameMs)) {
 277         // For full frame update, first clear the OSD completely
 278         uint8_t refreshSubcmd[1];
 279         refreshSubcmd[0] = MSP_DP_CLEAR_SCREEN;
 280         output(displayPort, MSP_DISPLAYPORT, refreshSubcmd, sizeof(refreshSubcmd));
 281
 282         // Then dirty the characters that are not blank, to send all data on this draw.
 283         for (unsigned int pos = 0; pos < sizeof(screen); pos++) {
 284             if (screen[pos] != SYM_BLANK) {
 285                 bitArraySet(dirty, pos);
 286             }
 287         }
 288
 289         sendSubFrameMs = (osdConfig()->msp_displayport_fullframe_interval > 0) ? (millis() + DS2MS(osdConfig()->msp_displayport_fullframe_interval)) : 0;
 290     }
 291
 292     uint8_t subcmd[COLS + 4];
 293     uint8_t updateCount = 0;
 294     subcmd[0] = MSP_DP_WRITE_STRING;
 295
 296     int next = BITARRAY_FIND_FIRST_SET(dirty, 0);
 297     while (next >= 0) {
 298         // Look for sequential dirty characters on the same line for the same font page
 299         int pos = next;
 300         uint8_t row = pos / COLS;
 301         uint8_t col = pos % COLS;
 302         uint8_t attributes = 0;
 303         int endOfLine = row * COLS + screenCols;
 304         uint8_t page = getAttrPage(attrs[pos]);
 305         uint8_t blink = getAttrBlink(attrs[pos]);
 306
 307         uint8_t len = 4;
 308         do {
 309             bitArrayClr(dirty, pos);
 310             subcmd[len] = isDJICompatibleVideoSystem(osdConfig()) ? getDJICharacter(screen[pos++], page): screen[pos++];
 311             len++;
 312
 313             if (bitArrayGet(dirty, pos)) {
 314                 next = pos;
 315             }
 316         } while (next == pos && next < endOfLine && getAttrPage(attrs[next]) == page && getAttrBlink(attrs[next]) == blink);
 317
 318         if (!isDJICompatibleVideoSystem(osdConfig())) {
 319             attributes |= (page << DISPLAYPORT_MSP_ATTR_FONTPAGE);
 320         }
 321
 322         if (blink) {
 323             attributes |= (1 << DISPLAYPORT_MSP_ATTR_BLINK);
 324         }
 325
 326         subcmd[1] = row;
 327         subcmd[2] = col;
 328         subcmd[3] = attributes;
 329         output(displayPort, MSP_DISPLAYPORT, subcmd, len);
 330         updateCount++;
 331         next = BITARRAY_FIND_FIRST_SET(dirty, pos);
 332     }
 333
 334     if (updateCount > 0 || screenCleared) {
 335         if (screenCleared) {
 336             screenCleared = false;
 337         }
 338
 339         subcmd[0] = MSP_DP_DRAW_SCREEN;
 340         output(displayPort, MSP_DISPLAYPORT, subcmd, 1);
 341     }
 342
 343     if (vtxReset) {
 344         clearScreen(displayPort);
 345         vtxReset = false;
 346     }
 347
 348     return 0;
 349 }
 350
 351 static void resync(displayPort_t *displayPort)
 352 {
 353     displayPort->rows = screenRows;
 354     displayPort->cols = screenCols;
 355 }
 356
 357 static int screenSize(const displayPort_t *displayPort)
 358 {
 359     return (displayPort->rows * displayPort->cols);
 360 }
 361
 362 static uint32_t txBytesFree(const displayPort_t *displayPort)
 363 {
 364     UNUSED(displayPort);
 365     return mspSerialTxBytesFree(mspPort.port);
 366 }
 367
 368 static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *displayPort)
 369 {
 370     UNUSED(displayPort);
 371     metadata->charCount = 1024;
 372     metadata->version = FONT_VERSION;
 373     return true;
 374 }
 375
 376 static textAttributes_t supportedTextAttributes(const displayPort_t *displayPort)
 377 {
 378     UNUSED(displayPort);
 379
 380     //textAttributes_t attr = TEXT_ATTRIBUTES_NONE;
 381     //TEXT_ATTRIBUTES_ADD_BLINK(attr);
 382     //return attr;
 383     return TEXT_ATTRIBUTES_NONE;
 384 }
 385
 386 static bool isTransferInProgress(const displayPort_t *displayPort)
 387 {
 388     UNUSED(displayPort);
 389     return false;
 390 }
 391
 392 static bool isReady(displayPort_t *displayPort)
 393 {
 394     UNUSED(displayPort);
 395     return vtxActive;
 396 }
 397
 398 static int heartbeat(displayPort_t *displayPort)
 399 {
 400     uint8_t subcmd[] = { MSP_DP_HEARTBEAT };
 401
 402     // heartbeat is used to:
 403     // a) ensure display is not released by MW OSD software
 404     // b) prevent OSD Slave boards from displaying a 'disconnected' status.
 405     return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
 406 }
 407
 408 static int grab(displayPort_t *displayPort)
 409 {
 410     return heartbeat(displayPort);
 411 }
 412
 413 static int release(displayPort_t *displayPort)
 414 {
 415     uint8_t subcmd[] = { MSP_DP_RELEASE };
 416     return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
 417 }
 418
 419 static const displayPortVTable_t mspOsdVTable = {
 420     .grab = grab,
 421     .release = release,
 422     .clearScreen = clearScreen,
 423     .drawScreen = drawScreen,
 424     .screenSize = screenSize,
 425     .writeString = writeString,
 426     .writeChar = writeChar,
 427     .readChar = readChar,
 428     .isTransferInProgress = isTransferInProgress,
 429     .heartbeat = heartbeat,
 430     .resync = resync,
 431     .txBytesFree = txBytesFree,
 432     .supportedTextAttributes = supportedTextAttributes,
 433     .getFontMetadata = getFontMetadata,
 434     .isReady = isReady,
 435 };
 436
 437 bool mspOsdSerialInit(void)
 438 {
 439     static volatile uint8_t txBuffer[TX_BUFFER_SIZE];
 440     memset(&mspPort, 0, sizeof(mspPort_t));
 441
 442     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD);
 443     if (portConfig) {
 444         serialPort_t *port = openSerialPort(portConfig->identifier, FUNCTION_MSP_OSD, NULL, NULL,
 445                 baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
 446
 447         if (port) {
 448             // Use a bigger TX buffer size to accommodate the configuration menus
 449             port->txBuffer = txBuffer;
 450             port->txBufferSize = TX_BUFFER_SIZE;
 451             port->txBufferTail = 0;
 452             port->txBufferHead = 0;
 453
 454             resetMspPort(&mspPort, port);
 455
 456             return true;
 457         }
 458     }
 459
 460     return false;
 461 }
 462
 463 displayPort_t* mspOsdDisplayPortInit(const videoSystem_e videoSystem)
 464 {
 465     if (mspOsdSerialInit()) {
 466         switch(videoSystem) {
 467         case VIDEO_SYSTEM_AUTO:
 468         case VIDEO_SYSTEM_DJICOMPAT:
 469         case VIDEO_SYSTEM_PAL:
 470             currentOsdMode = SD_3016;
 471             screenRows = PAL_ROWS;
 472             screenCols = PAL_COLS;
 473             break;
 474         case VIDEO_SYSTEM_NTSC:
 475             currentOsdMode = SD_3016;
 476             screenRows = NTSC_ROWS;
 477             screenCols = NTSC_COLS;
 478             break;
 479         case VIDEO_SYSTEM_HDZERO:
 480             currentOsdMode = HD_5018;
 481             screenRows = HDZERO_ROWS;
 482             screenCols = HDZERO_COLS;
 483             break;
 484         case VIDEO_SYSTEM_DJIWTF:
 485             currentOsdMode = HD_6022;
 486             screenRows = DJI_ROWS;
 487             screenCols = DJI_COLS;
 488             break;
 489         case VIDEO_SYSTEM_DJICOMPAT_HD:
 490         case VIDEO_SYSTEM_AVATAR:
 491             currentOsdMode = HD_5320;
 492             screenRows = AVATAR_ROWS;
 493             screenCols = AVATAR_COLS;
 494             break;
 495         default:
 496             break;
 497         }
 498
 499         osdVideoSystem = videoSystem;
 500         init();
 501         displayInit(&mspOsdDisplayPort, &mspOsdVTable);
 502
 503         if (osdVideoSystem == VIDEO_SYSTEM_DJICOMPAT) {
 504             mspOsdDisplayPort.displayPortType = "MSP DisplayPort: DJI Compatability mode";
 505         } else if (osdVideoSystem == VIDEO_SYSTEM_DJICOMPAT_HD) {
 506             mspOsdDisplayPort.displayPortType = "MSP DisplayPort: DJI Compatability mode (HD)";
 507         } else {
 508             mspOsdDisplayPort.displayPortType = "MSP DisplayPort";
 509         }
 510
 511         return &mspOsdDisplayPort;
 512     }
 513     return NULL;
 514 }
 515
 516 /*
 517  * Intercept MSP processor.
 518  * VTX sends an MSP command every 125ms or so.
 519  * VTX will have be marked as not ready if no commands received within VTX_TIMEOUT ms.
 520  */
 521 static mspResult_e processMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
 522 {
 523     if ((vtxSeen && !vtxActive) || (cmd->cmd == MSP_EEPROM_WRITE)) {
 524         vtxReset = true;
 525     }
 526
 527     vtxSeen = vtxActive = true;
 528     vtxHeartbeat = millis();
 529
 530     // Process MSP command
 531     return mspProcessCommand(cmd, reply, mspPostProcessFn);
 532 }
 533
 534 void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn)
 535 {
 536     if (mspPort.port) {
 537         mspProcessCommand = mspProcessCommandFn;
 538         mspSerialProcessOnePort(&mspPort, MSP_SKIP_NON_MSP_DATA, processMspCommand);
 539     }
 540 }
 541
 542 mspPort_t *getMspOsdPort(void)
 543 {
 544     if (mspPort.port) {
 545         return &mspPort;
 546     }
 547
 548     return NULL;
 549 }
 550
 551 #endif // USE_MSP_OSD