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 FILE_COMPILE_FOR_SPEED
  32
  33 #if defined(USE_OSD) && defined(USE_MSP_OSD)
  34
  35 #include "common/utils.h"
  36 #include "common/printf.h"
  37 #include "common/time.h"
  38 #include "common/bitarray.h"
  39
  40 #include "cms/cms.h"
  41
  42 #include "drivers/display.h"
  43 #include "drivers/display_font_metadata.h"
  44 #include "drivers/osd_symbols.h"
  45
  46 #include "fc/rc_modes.h"
  47
  48 #include "io/osd.h"
  49
  50 #include "msp/msp_protocol.h"
  51 #include "msp/msp_serial.h"
  52
  53 #include "displayport_msp_osd.h"
  54
  55 #include "displayport_msp_bf_compat.h"
  56
  57 #define FONT_VERSION 3
  58
  59 #define MSP_HEARTBEAT    0
  60 #define MSP_RELEASE      1
  61 #define MSP_CLEAR_SCREEN 2
  62 #define MSP_WRITE_STRING 3
  63 #define MSP_DRAW_SCREEN  4
  64 #define MSP_SET_OPTIONS  5
  65
  66 typedef enum {          // defines are from hdzero code
  67     SD_3016,
  68     HD_5018,
  69     HD_3016,           // Special HDZERO mode that just sends the centre 30x16 of the 50x18 canvas to the VRX
  70     HD_6022            // added to support DJI wtfos 60x22 grid
  71 } resolutionType_e;
  72
  73 #define DRAW_FREQ_DENOM 4 // 60Hz
  74 #define TX_BUFFER_SIZE 1024
  75 #define VTX_TIMEOUT 1000 // 1 second timer
  76
  77 static mspProcessCommandFnPtr mspProcessCommand;
  78 static mspPort_t mspPort;
  79 static displayPort_t mspOsdDisplayPort;
  80 static bool vtxSeen, vtxActive, vtxReset;
  81 static timeMs_t vtxHeartbeat;
  82
  83 // PAL screen size
  84 #define PAL_COLS 30
  85 #define PAL_ROWS 16
  86 // NTSC screen size
  87 #define NTSC_COLS 30
  88 #define NTSC_ROWS 13
  89 // HDZERO screen size
  90 #define HDZERO_COLS 50
  91 #define HDZERO_ROWS 18
  92 // Avatar screen size
  93 #define AVATAR_COLS 54
  94 #define AVATAR_ROWS 20
  95 // DJIWTF screen size
  96 #define DJI_COLS 60
  97 #define DJI_ROWS 22
  98
  99 // set COLS and ROWS to largest size available
 100 #define COLS DJI_COLS
 101 #define ROWS DJI_ROWS
 102
 103 // set screen size
 104 #define SCREENSIZE (ROWS*COLS)
 105
 106 static uint8_t currentOsdMode; // HDZero screen mode can change across layouts
 107
 108 static uint8_t screen[SCREENSIZE];
 109 static BITARRAY_DECLARE(fontPage, SCREENSIZE); // font page for each character on the screen
 110 static BITARRAY_DECLARE(dirty, SCREENSIZE); // change status for each character on the screen
 111 static bool screenCleared;
 112 static uint8_t screenRows, screenCols;
 113 static videoSystem_e osdVideoSystem;
 114
 115 extern uint8_t cliMode;
 116
 117 static void checkVtxPresent(void)
 118 {
 119     if (vtxActive && (millis()-vtxHeartbeat) > VTX_TIMEOUT) {
 120         vtxActive = false;
 121     }
 122 }
 123
 124 static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *subcmd, int len)
 125 {
 126     UNUSED(displayPort);
 127
 128     checkVtxPresent();
 129
 130     int sent = 0;
 131     if (!cliMode && vtxActive) {
 132         sent = mspSerialPushPort(cmd, subcmd, len, &mspPort, MSP_V1);
 133     }
 134
 135     return sent;
 136 }
 137
 138 static uint8_t determineHDZeroOsdMode(void)
 139 {
 140     if (cmsInMenu) {
 141         return HD_5018;
 142     }
 143
 144     // Check if all visible widgets are in the center 30x16 chars of the canvas.
 145     int activeLayout = osdGetActiveLayout(NULL);
 146     osd_items_e index = 0;
 147     do {
 148         index = osdIncElementIndex(index);
 149         uint16_t pos = osdLayoutsConfig()->item_pos[activeLayout][index];
 150         if (OSD_VISIBLE(pos)) {
 151             uint8_t elemPosX = OSD_X(pos);
 152             uint8_t elemPosY = OSD_Y(pos);
 153             if (!osdItemIsFixed(index) && (elemPosX < 10 || elemPosX > 39 || elemPosY == 0 || elemPosY == 17)) {
 154                 return HD_5018;
 155             }
 156         }
 157     } while (index > 0);
 158
 159     return HD_3016;
 160 }
 161
 162 static int setDisplayMode(displayPort_t *displayPort)
 163 {
 164     if (osdVideoSystem == VIDEO_SYSTEM_HDZERO) {
 165         currentOsdMode = determineHDZeroOsdMode(); // Can change between layouts
 166     }
 167
 168     uint8_t subcmd[] = { MSP_SET_OPTIONS, 0, currentOsdMode }; // Font selection, mode (SD/HD)
 169     return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
 170 }
 171
 172 static void init(void)
 173 {
 174     memset(screen, SYM_BLANK, sizeof(screen));
 175     BITARRAY_CLR_ALL(fontPage);
 176     BITARRAY_CLR_ALL(dirty);
 177 }
 178
 179 static int clearScreen(displayPort_t *displayPort)
 180 {
 181     uint8_t subcmd[] = { MSP_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_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)
 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             bitArraySet(dirty, pos);
 227         }
 228     }
 229     return 0;
 230 }
 231
 232 static int writeChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t c, textAttributes_t attr)
 233 {
 234     UNUSED(displayPort);
 235     UNUSED(attr);
 236
 237     return setChar((row * COLS) + col, c);
 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     UNUSED(attr);
 244
 245     uint16_t pos = (row * COLS) + col;
 246     while (*string) {
 247         setChar(pos++, *string++);
 248     }
 249     return 0;
 250 }
 251
 252 /**
 253  * Write only changed characters to the VTX
 254  */
 255 static int drawScreen(displayPort_t *displayPort) // 250Hz
 256 {
 257     static uint8_t counter = 0;
 258
 259     if ((!cmsInMenu && IS_RC_MODE_ACTIVE(BOXOSD)) || (counter++ % DRAW_FREQ_DENOM)) {
 260         return 0;
 261     }
 262
 263
 264     uint8_t subcmd[COLS + 4];
 265     uint8_t updateCount = 0;
 266     subcmd[0] = MSP_WRITE_STRING;
 267
 268     int next = BITARRAY_FIND_FIRST_SET(dirty, 0);
 269     while (next >= 0) {
 270         // Look for sequential dirty characters on the same line for the same font page
 271         int pos = next;
 272         uint8_t row = pos / COLS;
 273         uint8_t col = pos % COLS;
 274         int endOfLine = row * COLS + screenCols;
 275         bool page = bitArrayGet(fontPage, pos);
 276
 277         uint8_t len = 4;
 278         do {
 279             bitArrayClr(dirty, pos);
 280             subcmd[len++] = (osdVideoSystem == VIDEO_SYSTEM_BFCOMPAT) ? get_bf_character(screen[pos++], page): screen[pos++];
 281
 282             if (bitArrayGet(dirty, pos)) {
 283                 next = pos;
 284             }
 285         } while (next == pos && next < endOfLine && bitArrayGet(fontPage, next) == page);
 286
 287         subcmd[1] = row;
 288         subcmd[2] = col;
 289         subcmd[3] = (osdVideoSystem == VIDEO_SYSTEM_BFCOMPAT) ? 0 : page;
 290         output(displayPort, MSP_DISPLAYPORT, subcmd, len);
 291         updateCount++;
 292         next = BITARRAY_FIND_FIRST_SET(dirty, pos);
 293     }
 294
 295     if (updateCount > 0 || screenCleared) {
 296         if (screenCleared) {
 297             screenCleared = false;
 298         }
 299
 300         subcmd[0] = MSP_DRAW_SCREEN;
 301         output(displayPort, MSP_DISPLAYPORT, subcmd, 1);
 302     }
 303
 304     if (vtxReset) {
 305         clearScreen(displayPort);
 306         vtxReset = false;
 307     }
 308
 309 return 0;
 310 }
 311
 312 static void resync(displayPort_t *displayPort)
 313 {
 314     displayPort->rows = screenRows;
 315     displayPort->cols = screenCols;
 316 }
 317
 318 static int screenSize(const displayPort_t *displayPort)
 319 {
 320     return (displayPort->rows * displayPort->cols);
 321 }
 322
 323 static uint32_t txBytesFree(const displayPort_t *displayPort)
 324 {
 325     UNUSED(displayPort);
 326     return mspSerialTxBytesFree();
 327 }
 328
 329 static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *displayPort)
 330 {
 331     UNUSED(displayPort);
 332     metadata->charCount = 512;
 333     metadata->version = FONT_VERSION;
 334     return true;
 335 }
 336
 337 static textAttributes_t supportedTextAttributes(const displayPort_t *displayPort)
 338 {
 339     UNUSED(displayPort);
 340     return TEXT_ATTRIBUTES_NONE;
 341 }
 342
 343 static bool isTransferInProgress(const displayPort_t *displayPort)
 344 {
 345     UNUSED(displayPort);
 346     return false;
 347 }
 348
 349 static bool isReady(displayPort_t *displayPort)
 350 {
 351     UNUSED(displayPort);
 352     return vtxActive;
 353 }
 354
 355 static int heartbeat(displayPort_t *displayPort)
 356 {
 357     uint8_t subcmd[] = { MSP_HEARTBEAT };
 358     return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
 359 }
 360
 361 static int grab(displayPort_t *displayPort)
 362 {
 363     return heartbeat(displayPort);
 364 }
 365
 366 static int release(displayPort_t *displayPort)
 367 {
 368     uint8_t subcmd[] = { MSP_RELEASE };
 369     return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
 370 }
 371
 372 static const displayPortVTable_t mspOsdVTable = {
 373     .grab = grab,
 374     .release = release,
 375     .clearScreen = clearScreen,
 376     .drawScreen = drawScreen,
 377     .screenSize = screenSize,
 378     .writeString = writeString,
 379     .writeChar = writeChar,
 380     .readChar = readChar,
 381     .isTransferInProgress = isTransferInProgress,
 382     .heartbeat = heartbeat,
 383     .resync = resync,
 384     .txBytesFree = txBytesFree,
 385     .supportedTextAttributes = supportedTextAttributes,
 386     .getFontMetadata = getFontMetadata,
 387     .isReady = isReady,
 388 };
 389
 390 bool mspOsdSerialInit(void)
 391 {
 392     static volatile uint8_t txBuffer[TX_BUFFER_SIZE];
 393     memset(&mspPort, 0, sizeof(mspPort_t));
 394
 395     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD);
 396     if (portConfig) {
 397         serialPort_t *port = openSerialPort(portConfig->identifier, FUNCTION_MSP_OSD, NULL, NULL,
 398                 baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
 399
 400         if (port) {
 401             // Use a bigger TX buffer size to accommodate the configuration menus
 402             port->txBuffer = txBuffer;
 403             port->txBufferSize = TX_BUFFER_SIZE;
 404             port->txBufferTail = 0;
 405             port->txBufferHead = 0;
 406
 407             resetMspPort(&mspPort, port);
 408
 409             return true;
 410         }
 411     }
 412
 413     return false;
 414 }
 415
 416 displayPort_t* mspOsdDisplayPortInit(const videoSystem_e videoSystem)
 417 {
 418     if (mspOsdSerialInit()) {
 419         switch(videoSystem) {
 420         case VIDEO_SYSTEM_AUTO:
 421         case VIDEO_SYSTEM_BFCOMPAT:
 422         case VIDEO_SYSTEM_PAL:
 423             currentOsdMode = SD_3016;
 424             screenRows = PAL_ROWS;
 425             screenCols = PAL_COLS;
 426             break;
 427         case VIDEO_SYSTEM_NTSC:
 428             currentOsdMode = SD_3016;
 429             screenRows = NTSC_ROWS;
 430             screenCols = NTSC_COLS;
 431             break;
 432         case VIDEO_SYSTEM_HDZERO:
 433             currentOsdMode = HD_5018;
 434             screenRows = HDZERO_ROWS;
 435             screenCols = HDZERO_COLS;
 436             break;
 437         case VIDEO_SYSTEM_DJIWTF:
 438             currentOsdMode = HD_6022;
 439             screenRows = DJI_ROWS;
 440             screenCols = DJI_COLS;
 441             break;
 442         default:
 443             break;
 444         }
 445
 446         osdVideoSystem = videoSystem;
 447         init();
 448         displayInit(&mspOsdDisplayPort, &mspOsdVTable);
 449
 450         return &mspOsdDisplayPort;
 451     }
 452     return NULL;
 453 }
 454
 455 /*
 456  * Intercept MSP processor.
 457  * VTX sends an MSP command every 125ms or so.
 458  * VTX will have be marked as not ready if no commands received within VTX_TIMEOUT ms.
 459  */
 460 static mspResult_e processMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
 461 {
 462     if ((vtxSeen && !vtxActive) || (cmd->cmd == MSP_EEPROM_WRITE)) {
 463         vtxReset = true;
 464     }
 465
 466     vtxSeen = vtxActive = true;
 467     vtxHeartbeat = millis();
 468
 469     // Process MSP command
 470     return mspProcessCommand(cmd, reply, mspPostProcessFn);
 471 }
 472
 473 void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn)
 474 {
 475     if (mspPort.port) {
 476         mspProcessCommand = mspProcessCommandFn;
 477         mspSerialProcessOnePort(&mspPort, MSP_SKIP_NON_MSP_DATA, processMspCommand);
 478     }
 479 }
 480
 481 #endif // USE_MSP_OSD