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_bf_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 BITARRAY_DECLARE(fontPage, SCREENSIZE);  // font page for each character on the screen
 104 static BITARRAY_DECLARE(dirty, SCREENSIZE);     // change status for each character on the screen
 105 static BITARRAY_DECLARE(blinkChar, SCREENSIZE); // Does the character blink?
 106 static bool screenCleared;
 107 static uint8_t screenRows, screenCols;
 108 static videoSystem_e osdVideoSystem;
 109
 110 extern uint8_t cliMode;
 111
 112 static void checkVtxPresent(void)
 113 {
 114     if (vtxActive && (millis()-vtxHeartbeat) > VTX_TIMEOUT) {
 115         vtxActive = false;
 116     }
 117
 118     if (ARMING_FLAG(SIMULATOR_MODE_HITL)) {
 119         vtxActive = true;
 120     }
 121 }
 122
 123 static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *subcmd, int len)
 124 {
 125     UNUSED(displayPort);
 126
 127     checkVtxPresent();
 128
 129     int sent = 0;
 130     if (!cliMode && vtxActive) {
 131         sent = mspSerialPushPort(cmd, subcmd, len, &mspPort, MSP_V1);
 132     }
 133
 134     return sent;
 135 }
 136
 137 static uint8_t determineHDZeroOsdMode(void)
 138 {
 139     if (cmsInMenu) {
 140         return HD_5018;
 141     }
 142
 143     // Check if all visible widgets are in the center 30x16 chars of the canvas.
 144     int activeLayout = osdGetActiveLayout(NULL);
 145     osd_items_e index = 0;
 146     do {
 147         index = osdIncElementIndex(index);
 148         uint16_t pos = osdLayoutsConfig()->item_pos[activeLayout][index];
 149         if (OSD_VISIBLE(pos)) {
 150             uint8_t elemPosX = OSD_X(pos);
 151             uint8_t elemPosY = OSD_Y(pos);
 152             if (!osdItemIsFixed(index) && (elemPosX < 10 || elemPosX > 39 || elemPosY == 0 || elemPosY == 17)) {
 153                 return HD_5018;
 154             }
 155         }
 156     } while (index > 0);
 157
 158     return HD_3016;
 159 }
 160
 161 static int setDisplayMode(displayPort_t *displayPort)
 162 {
 163     if (osdVideoSystem == VIDEO_SYSTEM_HDZERO) {
 164         currentOsdMode = determineHDZeroOsdMode(); // Can change between layouts
 165     }
 166
 167     uint8_t subcmd[] = { MSP_DP_OPTIONS, 0, currentOsdMode }; // Font selection, mode (SD/HD)
 168     return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
 169 }
 170
 171 static void init(void)
 172 {
 173     memset(screen, SYM_BLANK, sizeof(screen));
 174     BITARRAY_CLR_ALL(fontPage);
 175     BITARRAY_CLR_ALL(dirty);
 176     BITARRAY_CLR_ALL(blinkChar);
 177 }
 178
 179 static int clearScreen(displayPort_t *displayPort)
 180 {
 181     uint8_t subcmd[] = { MSP_DP_CLEAR_SCREEN };
 182
 183     if (!cmsInMenu && IS_RC_MODE_ACTIVE(BOXOSD)) { // OSD is off
 184         output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
 185         subcmd[0] = MSP_DP_DRAW_SCREEN;
 186         vtxReset = true;
 187     }
 188     else {
 189         init();
 190         setDisplayMode(displayPort);
 191         screenCleared = true;
 192     }
 193
 194     return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
 195 }
 196
 197 static bool readChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t *c, textAttributes_t *attr)
 198 {
 199     UNUSED(displayPort);
 200
 201     uint16_t pos = (row * COLS) + col;
 202     if (pos >= SCREENSIZE) {
 203         return false;
 204     }
 205
 206     *c = screen[pos];
 207     if (bitArrayGet(fontPage, pos)) {
 208         *c |= 0x100;
 209     }
 210
 211     if (attr) {
 212         *attr = TEXT_ATTRIBUTES_NONE;
 213     }
 214
 215     return true;
 216 }
 217
 218 static int setChar(const uint16_t pos, const uint16_t c, textAttributes_t attr)
 219 {
 220     if (pos < SCREENSIZE) {
 221         uint8_t ch = c & 0xFF;
 222         bool page = (c >> 8);
 223         if (screen[pos] != ch || bitArrayGet(fontPage, pos) != page) {
 224             screen[pos] = ch;
 225             (page) ? bitArraySet(fontPage, pos) : bitArrayClr(fontPage, pos);
 226             (TEXT_ATTRIBUTES_HAVE_BLINK(attr)) ? bitArraySet(blinkChar, pos) : bitArrayClr(blinkChar, pos);
 227             bitArraySet(dirty, pos);
 228         }
 229     }
 230     return 0;
 231 }
 232
 233 static int writeChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t c, textAttributes_t attr)
 234 {
 235     UNUSED(displayPort);
 236
 237     return setChar((row * COLS) + col, c, attr);
 238 }
 239
 240 static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string, textAttributes_t attr)
 241 {
 242     UNUSED(displayPort);
 243
 244     uint16_t pos = (row * COLS) + col;
 245     while (*string) {
 246         setChar(pos++, *string++, attr);
 247     }
 248     return 0;
 249 }
 250
 251 /**
 252  * Write only changed characters to the VTX
 253  */
 254 static int drawScreen(displayPort_t *displayPort) // 250Hz
 255 {
 256     static uint8_t counter = 0;
 257
 258     if ((!cmsInMenu && IS_RC_MODE_ACTIVE(BOXOSD)) || (counter++ % DRAW_FREQ_DENOM)) { // 62.5Hz
 259         return 0;
 260     }
 261
 262     if (osdConfig()->msp_displayport_fullframe_interval >= 0 && (millis() > sendSubFrameMs)) {
 263         // For full frame update, first clear the OSD completely
 264         uint8_t refreshSubcmd[1];
 265         refreshSubcmd[0] = MSP_DP_CLEAR_SCREEN;
 266         output(displayPort, MSP_DISPLAYPORT, refreshSubcmd, sizeof(refreshSubcmd));
 267
 268         // Then dirty the characters that are not blank, to send all data on this draw.
 269         for (unsigned int pos = 0; pos < sizeof(screen); pos++) {
 270             if (screen[pos] != SYM_BLANK) {
 271                 bitArraySet(dirty, pos);
 272             }
 273         }
 274
 275         sendSubFrameMs = (osdConfig()->msp_displayport_fullframe_interval > 0) ? (millis() + DS2MS(osdConfig()->msp_displayport_fullframe_interval)) : 0;
 276     }
 277
 278     uint8_t subcmd[COLS + 4];
 279     uint8_t updateCount = 0;
 280     subcmd[0] = MSP_DP_WRITE_STRING;
 281
 282     int next = BITARRAY_FIND_FIRST_SET(dirty, 0);
 283     while (next >= 0) {
 284         // Look for sequential dirty characters on the same line for the same font page
 285         int pos = next;
 286         uint8_t row = pos / COLS;
 287         uint8_t col = pos % COLS;
 288         uint8_t attributes = 0;
 289         int endOfLine = row * COLS + screenCols;
 290         bool page = bitArrayGet(fontPage, pos);
 291         bool blink = bitArrayGet(blinkChar, pos);
 292
 293         uint8_t len = 4;
 294         do {
 295             bitArrayClr(dirty, pos);
 296             subcmd[len] = isBfCompatibleVideoSystem(osdConfig()) ? getBfCharacter(screen[pos++], page): screen[pos++];
 297             len++;
 298
 299             if (bitArrayGet(dirty, pos)) {
 300                 next = pos;
 301             }
 302         } while (next == pos && next < endOfLine && bitArrayGet(fontPage, next) == page && bitArrayGet(blinkChar, next) == blink);
 303
 304         if (!isBfCompatibleVideoSystem(osdConfig())) {
 305             attributes |= (page << DISPLAYPORT_MSP_ATTR_FONTPAGE);
 306         }
 307
 308         if (blink) {
 309             attributes |= (1 << DISPLAYPORT_MSP_ATTR_BLINK);
 310         }
 311
 312         subcmd[1] = row;
 313         subcmd[2] = col;
 314         subcmd[3] = attributes;
 315         output(displayPort, MSP_DISPLAYPORT, subcmd, len);
 316         updateCount++;
 317         next = BITARRAY_FIND_FIRST_SET(dirty, pos);
 318     }
 319
 320     if (updateCount > 0 || screenCleared) {
 321         if (screenCleared) {
 322             screenCleared = false;
 323         }
 324
 325         subcmd[0] = MSP_DP_DRAW_SCREEN;
 326         output(displayPort, MSP_DISPLAYPORT, subcmd, 1);
 327     }
 328
 329     if (vtxReset) {
 330         clearScreen(displayPort);
 331         vtxReset = false;
 332     }
 333
 334     return 0;
 335 }
 336
 337 static void resync(displayPort_t *displayPort)
 338 {
 339     displayPort->rows = screenRows;
 340     displayPort->cols = screenCols;
 341 }
 342
 343 static int screenSize(const displayPort_t *displayPort)
 344 {
 345     return (displayPort->rows * displayPort->cols);
 346 }
 347
 348 static uint32_t txBytesFree(const displayPort_t *displayPort)
 349 {
 350     UNUSED(displayPort);
 351     return mspSerialTxBytesFree(mspPort.port);
 352 }
 353
 354 static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *displayPort)
 355 {
 356     UNUSED(displayPort);
 357     metadata->charCount = 512;
 358     metadata->version = FONT_VERSION;
 359     return true;
 360 }
 361
 362 static textAttributes_t supportedTextAttributes(const displayPort_t *displayPort)
 363 {
 364     UNUSED(displayPort);
 365
 366     //textAttributes_t attr = TEXT_ATTRIBUTES_NONE;
 367     //TEXT_ATTRIBUTES_ADD_BLINK(attr);
 368     //return attr;
 369     return TEXT_ATTRIBUTES_NONE;
 370 }
 371
 372 static bool isTransferInProgress(const displayPort_t *displayPort)
 373 {
 374     UNUSED(displayPort);
 375     return false;
 376 }
 377
 378 static bool isReady(displayPort_t *displayPort)
 379 {
 380     UNUSED(displayPort);
 381     return vtxActive;
 382 }
 383
 384 static int heartbeat(displayPort_t *displayPort)
 385 {
 386     uint8_t subcmd[] = { MSP_DP_HEARTBEAT };
 387
 388     // heartbeat is used to:
 389     // a) ensure display is not released by MW OSD software
 390     // b) prevent OSD Slave boards from displaying a 'disconnected' status.
 391     return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
 392 }
 393
 394 static int grab(displayPort_t *displayPort)
 395 {
 396     return heartbeat(displayPort);
 397 }
 398
 399 static int release(displayPort_t *displayPort)
 400 {
 401     uint8_t subcmd[] = { MSP_DP_RELEASE };
 402     return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
 403 }
 404
 405 static const displayPortVTable_t mspOsdVTable = {
 406     .grab = grab,
 407     .release = release,
 408     .clearScreen = clearScreen,
 409     .drawScreen = drawScreen,
 410     .screenSize = screenSize,
 411     .writeString = writeString,
 412     .writeChar = writeChar,
 413     .readChar = readChar,
 414     .isTransferInProgress = isTransferInProgress,
 415     .heartbeat = heartbeat,
 416     .resync = resync,
 417     .txBytesFree = txBytesFree,
 418     .supportedTextAttributes = supportedTextAttributes,
 419     .getFontMetadata = getFontMetadata,
 420     .isReady = isReady,
 421 };
 422
 423 bool mspOsdSerialInit(void)
 424 {
 425     static volatile uint8_t txBuffer[TX_BUFFER_SIZE];
 426     memset(&mspPort, 0, sizeof(mspPort_t));
 427
 428     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD);
 429     if (portConfig) {
 430         serialPort_t *port = openSerialPort(portConfig->identifier, FUNCTION_MSP_OSD, NULL, NULL,
 431                 baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
 432
 433         if (port) {
 434             // Use a bigger TX buffer size to accommodate the configuration menus
 435             port->txBuffer = txBuffer;
 436             port->txBufferSize = TX_BUFFER_SIZE;
 437             port->txBufferTail = 0;
 438             port->txBufferHead = 0;
 439
 440             resetMspPort(&mspPort, port);
 441
 442             return true;
 443         }
 444     }
 445
 446     return false;
 447 }
 448
 449 displayPort_t* mspOsdDisplayPortInit(const videoSystem_e videoSystem)
 450 {
 451     if (mspOsdSerialInit()) {
 452         switch(videoSystem) {
 453         case VIDEO_SYSTEM_AUTO:
 454         case VIDEO_SYSTEM_BFCOMPAT:
 455         case VIDEO_SYSTEM_PAL:
 456             currentOsdMode = SD_3016;
 457             screenRows = PAL_ROWS;
 458             screenCols = PAL_COLS;
 459             break;
 460         case VIDEO_SYSTEM_NTSC:
 461             currentOsdMode = SD_3016;
 462             screenRows = NTSC_ROWS;
 463             screenCols = NTSC_COLS;
 464             break;
 465         case VIDEO_SYSTEM_HDZERO:
 466             currentOsdMode = HD_5018;
 467             screenRows = HDZERO_ROWS;
 468             screenCols = HDZERO_COLS;
 469             break;
 470         case VIDEO_SYSTEM_DJIWTF:
 471             currentOsdMode = HD_6022;
 472             screenRows = DJI_ROWS;
 473             screenCols = DJI_COLS;
 474             break;
 475         case VIDEO_SYSTEM_BFCOMPAT_HD:
 476         case VIDEO_SYSTEM_AVATAR:
 477             currentOsdMode = HD_5320;
 478             screenRows = AVATAR_ROWS;
 479             screenCols = AVATAR_COLS;
 480             break;
 481         default:
 482             break;
 483         }
 484
 485         osdVideoSystem = videoSystem;
 486         init();
 487         displayInit(&mspOsdDisplayPort, &mspOsdVTable);
 488
 489         if (osdVideoSystem == VIDEO_SYSTEM_BFCOMPAT) {
 490             mspOsdDisplayPort.displayPortType = "MSP DisplayPort: BetaFlight Compatability mode";
 491         } else if (osdVideoSystem == VIDEO_SYSTEM_BFCOMPAT_HD) {
 492             mspOsdDisplayPort.displayPortType = "MSP DisplayPort: BetaFlight Compatability mode (HD)";
 493         } else {
 494             mspOsdDisplayPort.displayPortType = "MSP DisplayPort";
 495         }
 496
 497         return &mspOsdDisplayPort;
 498     }
 499     return NULL;
 500 }
 501
 502 /*
 503  * Intercept MSP processor.
 504  * VTX sends an MSP command every 125ms or so.
 505  * VTX will have be marked as not ready if no commands received within VTX_TIMEOUT ms.
 506  */
 507 static mspResult_e processMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
 508 {
 509     if ((vtxSeen && !vtxActive) || (cmd->cmd == MSP_EEPROM_WRITE)) {
 510         vtxReset = true;
 511     }
 512
 513     vtxSeen = vtxActive = true;
 514     vtxHeartbeat = millis();
 515
 516     // Process MSP command
 517     return mspProcessCommand(cmd, reply, mspPostProcessFn);
 518 }
 519
 520 void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn)
 521 {
 522     if (mspPort.port) {
 523         mspProcessCommand = mspProcessCommandFn;
 524         mspSerialProcessOnePort(&mspPort, MSP_SKIP_NON_MSP_DATA, processMspCommand);
 525     }
 526 }
 527
 528 mspPort_t *getMspOsdPort()
 529 {
 530     if (mspPort.port) {
 531         return &mspPort;
 532     }
 533
 534     return NULL;
 535 }
 536
 537 #endif // USE_MSP_OSD