src/main/io/frsky_osd.c

   1 #include <stddef.h>
   2 #include <stdint.h>
   3 #include <string.h>
   4
   5 #include "platform.h"
   6
   7 #if defined(USE_FRSKYOSD)
   8
   9 #include "common/crc.h"
  10 #include "common/maths.h"
  11 #include "common/time.h"
  12 #include "common/utils.h"
  13 #include "common/uvarint.h"
  14
  15 #include "drivers/time.h"
  16
  17 #include "io/frsky_osd.h"
  18 #include "io/serial.h"
  19
  20 #define FRSKY_OSD_BAUDRATE 115200
  21 #define FRSKY_OSD_SUPPORTED_API_VERSION 1
  22
  23 #define FRSKY_OSD_PREAMBLE_BYTE_0 '$'
  24 #define FRSKY_OSD_PREAMBLE_BYTE_1 'A'
  25
  26 #define FRSKY_OSD_GRID_BUFFER_CHAR_BITS 9
  27 #define FRSKY_OSD_GRID_BUFFER_CHAR_MASK ((1 << FRSKY_OSD_GRID_BUFFER_CHAR_BITS) - 1)
  28 #define FRSKY_OSD_GRID_BUFFER_ENCODE(chr, attr) ((chr & FRSKY_OSD_GRID_BUFFER_CHAR_MASK) | (attr << FRSKY_OSD_GRID_BUFFER_CHAR_BITS))
  29
  30 #define FRSKY_OSD_CHAR_ATTRIBUTE_COLOR_INVERSE (1 << 0)
  31 #define FRSKY_OSD_CHAR_ATTRIBUTE_SOLID_BACKGROUND (1 << 1)
  32
  33 #define FRSKY_OSD_CHAR_DATA_BYTES 54
  34 #define FRSKY_OSD_CHAR_METADATA_BYTES 10
  35 #define FRSKY_OSD_CHAR_TOTAL_BYTES (FRSKY_OSD_CHAR_DATA_BYTES + FRSKY_OSD_CHAR_METADATA_BYTES)
  36
  37 #define FRSKY_OSD_SEND_BUFFER_SIZE 192
  38 #define FRSKY_OSD_RECV_BUFFER_SIZE 128
  39
  40 #define FRSKY_OSD_CMD_RESPONSE_ERROR 0
  41
  42 #define FRSKY_OSD_INFO_INTERVAL_MS 1000
  43
  44 #define FRSKY_OSD_TRACE(...)
  45 #define FRSKY_OSD_DEBUG(...)
  46 #define FRSKY_OSD_ERROR(...)
  47 #define FRSKY_OSD_ASSERT(x)
  48
  49 typedef enum
  50 {
  51     OSD_CMD_RESPONSE_ERROR = 0,
  52
  53     OSD_CMD_INFO = 1,
  54     OSD_CMD_READ_FONT = 2,
  55     OSD_CMD_WRITE_FONT = 3,
  56     OSD_CMD_GET_CAMERA = 4,
  57     OSD_CMD_SET_CAMERA = 5,
  58     OSD_CMD_GET_ACTIVE_CAMERA = 6,
  59     OSD_CMD_GET_OSD_ENABLED = 7,
  60     OSD_CMD_SET_OSD_ENABLED = 8,
  61
  62     OSD_CMD_TRANSACTION_BEGIN = 16,
  63     OSD_CMD_TRANSACTION_COMMIT = 17,
  64     OSD_CMD_TRANSACTION_BEGIN_PROFILED = 18,
  65     OSD_CMD_TRANSACTION_BEGIN_RESET_DRAWING = 19,
  66
  67     OSD_CMD_DRAWING_SET_STROKE_COLOR = 22,
  68     OSD_CMD_DRAWING_SET_FILL_COLOR = 23,
  69     OSD_CMD_DRAWING_SET_STROKE_AND_FILL_COLOR = 24,
  70     OSD_CMD_DRAWING_SET_COLOR_INVERSION = 25,
  71     OSD_CMD_DRAWING_SET_PIXEL = 26,
  72     OSD_CMD_DRAWING_SET_PIXEL_TO_STROKE_COLOR = 27,
  73     OSD_CMD_DRAWING_SET_PIXEL_TO_FILL_COLOR = 28,
  74     OSD_CMD_DRAWING_SET_STROKE_WIDTH = 29,
  75     OSD_CMD_DRAWING_SET_LINE_OUTLINE_TYPE = 30,
  76     OSD_CMD_DRAWING_SET_LINE_OUTLINE_COLOR = 31,
  77
  78     OSD_CMD_DRAWING_CLIP_TO_RECT = 40,
  79     OSD_CMD_DRAWING_CLEAR_SCREEN = 41,
  80     OSD_CMD_DRAWING_CLEAR_RECT = 42,
  81     OSD_CMD_DRAWING_RESET = 43,
  82     OSD_CMD_DRAWING_DRAW_BITMAP = 44,
  83     OSD_CMD_DRAWING_DRAW_BITMAP_MASK = 45,
  84     OSD_CMD_DRAWING_DRAW_CHAR = 46,
  85     OSD_CMD_DRAWING_DRAW_CHAR_MASK = 47,
  86     OSD_CMD_DRAWING_DRAW_STRING = 48,
  87     OSD_CMD_DRAWING_DRAW_STRING_MASK = 49,
  88     OSD_CMD_DRAWING_MOVE_TO_POINT = 50,
  89     OSD_CMD_DRAWING_STROKE_LINE_TO_POINT = 51,
  90     OSD_CMD_DRAWING_STROKE_TRIANGLE = 52,
  91     OSD_CMD_DRAWING_FILL_TRIANGLE = 53,
  92     OSD_CMD_DRAWING_FILL_STROKE_TRIANGLE = 54,
  93     OSD_CMD_DRAWING_STROKE_RECT = 55,
  94     OSD_CMD_DRAWING_FILL_RECT = 56,
  95     OSD_CMD_DRAWING_FILL_STROKE_RECT = 57,
  96     OSD_CMD_DRAWING_STROKE_ELLIPSE_IN_RECT = 58,
  97     OSD_CMD_DRAWING_FILL_ELLIPSE_IN_RECT = 59,
  98     OSD_CMD_DRAWING_FILL_STROKE_ELLIPSE_IN_RECT = 60,
  99
 100     OSD_CMD_CTM_RESET = 80,
 101     OSD_CMD_CTM_SET = 81,
 102     OSD_CMD_CTM_TRANSLATE = 82,
 103     OSD_CMD_CTM_SCALE = 83,
 104     OSD_CMD_CTM_ROTATE = 84,
 105     OSD_CMD_CTM_ROTATE_ABOUT = 85,
 106     OSD_CMD_CTM_SHEAR = 86,
 107     OSD_CMD_CTM_SHEAR_ABOUT = 87,
 108     OSD_CMD_CTM_MULTIPLY = 88,
 109
 110     OSD_CMD_CONTEXT_PUSH = 100,
 111     OSD_CMD_CONTEXT_POP = 101,
 112
 113     // MAX7456 emulation commands
 114     OSD_CMD_DRAW_GRID_CHR = 110,
 115     OSD_CMD_DRAW_GRID_STR = 111,
 116 } osdCommand_e;
 117
 118 typedef enum {
 119     RECV_STATE_NONE,
 120     RECV_STATE_SYNC,
 121     RECV_STATE_LENGTH,
 122     RECV_STATE_DATA,
 123     RECV_STATE_CHECKSUM,
 124     RECV_STATE_DONE,
 125 } frskyOsdRecvState_e;
 126
 127 typedef struct frskyOsdInfoResponse_s {
 128     uint8_t magic[3];
 129     uint8_t versionMajor;
 130     uint8_t versionMinor;
 131     uint8_t versionPatch;
 132     uint8_t gridRows;
 133     uint8_t gridColumns;
 134     uint16_t pixelWidth;
 135     uint16_t pixelHeight;
 136     uint8_t tvStandard;
 137     uint8_t hasDetectedCamera;
 138     uint16_t maxFrameSize;
 139     uint8_t contextStackSize;
 140 } __attribute__((packed)) frskyOsdInfoResponse_t;
 141
 142 typedef struct frskyOsdFontCharacter_s {
 143     uint16_t addr;
 144     struct {
 145         uint8_t bitmap[FRSKY_OSD_CHAR_DATA_BYTES]; // 12x18 2bpp
 146         uint8_t metadata[FRSKY_OSD_CHAR_METADATA_BYTES];
 147     } data;
 148 } __attribute__((packed)) frskyOsdCharacter_t;
 149
 150 typedef struct frskyOsdDrawGridCharCmd_s {
 151     uint8_t gx;
 152     uint8_t gy;
 153     uint16_t chr;
 154     uint8_t opts;
 155 } __attribute__((packed)) frskyOsdDrawGridCharCmd_t;
 156
 157 typedef struct frskyOsdDrawGridStrHeaderCmd_s {
 158     uint8_t gx;
 159     uint8_t gy;
 160     uint8_t opts;
 161     // uvarint with size and blob folow
 162 } __attribute__((packed)) frskyOsdDrawGridStrHeaderCmd_t;
 163
 164 typedef struct frskyOsdPoint_s {
 165     int x : 12;
 166     int y : 12;
 167 } __attribute__((packed)) frskyOsdPoint_t;
 168
 169 typedef struct frskyOsdSize_s {
 170     int w : 12;
 171     int h : 12;
 172 } __attribute__((packed)) frskyOsdSize_t;
 173
 174 typedef struct frskyOsdRect_s {
 175     frskyOsdPoint_t origin;
 176     frskyOsdSize_t size;
 177 } __attribute__((packed)) frskyOsdRect_t;
 178
 179 typedef struct frskyOsdTriangle_s {
 180     frskyOsdPoint_t p1;
 181     frskyOsdPoint_t p2;
 182     frskyOsdPoint_t p3;
 183 } __attribute__((packed)) frskyOsdTriangle_t;
 184
 185 typedef struct frskyOsdSetPixel_s {
 186     frskyOsdPoint_t p;
 187     uint8_t color;
 188 }  __attribute__((packed)) frskyOsdSetPixel_t;
 189
 190 typedef struct frskyOsdDrawCharacterCmd_s {
 191     frskyOsdPoint_t p;
 192     uint16_t chr;
 193     uint8_t opts;
 194 }  __attribute__((packed)) frskyOsdDrawCharacterCmd_t;
 195
 196 typedef struct frskyOsdDrawCharacterMaskCmd_s {
 197     frskyOsdDrawCharacterCmd_t dc;
 198     uint8_t maskColor;
 199 }  __attribute__((packed)) frskyOsdDrawCharacterMaskCmd_t;
 200
 201 typedef struct frskyOsdDrawStrCommandHeaderCmd_s {
 202     frskyOsdPoint_t p;
 203     uint8_t opts;
 204     // uvarint with size and blob follow
 205 } __attribute__((packed)) frskyOsdDrawStrCommandHeaderCmd_t;
 206
 207 typedef struct frskyOsdDrawStrMaskCommandHeaderCmd_s {
 208     frskyOsdPoint_t p;
 209     uint8_t opts;
 210     uint8_t maskColor;
 211     // uvarint with size and blob follow
 212 } __attribute__((packed)) frskyOsdDrawStrMaskCommandHeaderCmd_t;
 213
 214
 215 typedef struct frskyOsdState_s {
 216     struct {
 217         uint8_t data[FRSKY_OSD_SEND_BUFFER_SIZE];
 218         uint8_t pos;
 219     } sendBuffer;
 220     struct {
 221         uint8_t state;
 222         uint8_t crc;
 223         uint16_t expected;
 224         uint8_t expectedShift;
 225         uint8_t data[FRSKY_OSD_RECV_BUFFER_SIZE];
 226         uint8_t pos;
 227     } recvBuffer;
 228     struct {
 229         uint8_t major;
 230         uint8_t minor;
 231         timeMs_t nextRequest;
 232         struct {
 233             uint8_t rows;
 234             uint8_t columns;
 235         } grid;
 236         struct {
 237             uint16_t width;
 238             uint16_t height;
 239         } viewport;
 240     } info;
 241     struct {
 242         uint16_t addr;
 243         osdCharacter_t *chr;
 244     } recvOsdCharacter;
 245     serialPort_t *port;
 246     bool initialized;
 247     timeMs_t nextInfoRequest;
 248 } frskyOsdState_t;
 249
 250 static frskyOsdState_t state;
 251
 252 static uint8_t frskyOsdChecksum(uint8_t crc, uint8_t c)
 253 {
 254     return crc8_dvb_s2(crc, c);
 255 }
 256
 257 static void frskyOsdResetReceiveBuffer(void)
 258 {
 259     state.recvBuffer.state = RECV_STATE_NONE;
 260     state.recvBuffer.crc = 0;
 261     state.recvBuffer.expected = 0;
 262     state.recvBuffer.expectedShift = 0;
 263     state.recvBuffer.pos = 0;
 264 }
 265
 266 static void frskyOsdResetSendBuffer(void)
 267 {
 268     state.sendBuffer.pos = 0;
 269 }
 270
 271 static void frskyOsdProcessCommandU8(uint8_t *crc, uint8_t c)
 272 {
 273     while (serialTxBytesFree(state.port) == 0) {
 274     };
 275     serialWrite(state.port, c);
 276     if (crc) {
 277         *crc = crc8_dvb_s2(*crc, c);
 278     }
 279 }
 280
 281 static void frskyOsdSendCommand(uint8_t cmd, const void *payload, size_t size)
 282 {
 283     int required = size + 1;
 284     FRSKY_OSD_ASSERT(required <= sizeof(state.sendBuffer.data));
 285     int rem = sizeof(state.sendBuffer.data) - state.sendBuffer.pos;
 286     if (rem < required) {
 287         frskyOsdFlushSendBuffer();
 288     }
 289     state.sendBuffer.data[state.sendBuffer.pos++] = cmd;
 290     const uint8_t *ptr = payload;
 291     for (size_t ii = 0; ii < size; ii++, ptr++) {
 292         state.sendBuffer.data[state.sendBuffer.pos++] = *ptr;
 293     }
 294 }
 295
 296 static void frskyOsdStateReset(serialPort_t *port)
 297 {
 298     frskyOsdResetReceiveBuffer();
 299     frskyOsdResetSendBuffer();
 300     state.info.grid.rows = 0;
 301     state.info.grid.columns = 0;
 302     state.info.viewport.width = 0;
 303     state.info.viewport.height = 0;
 304
 305     state.port = port;
 306     state.initialized = false;
 307 }
 308
 309 static void frskyOsdUpdateReceiveBuffer(void)
 310 {
 311     while (serialRxBytesWaiting(state.port) > 0) {
 312         uint8_t c = serialRead(state.port);
 313         switch ((frskyOsdRecvState_e)state.recvBuffer.state) {
 314         case RECV_STATE_NONE:
 315             if (c != FRSKY_OSD_PREAMBLE_BYTE_0) {
 316                 break;
 317             }
 318             state.recvBuffer.state = RECV_STATE_SYNC;
 319             break;
 320         case RECV_STATE_SYNC:
 321             if (c != FRSKY_OSD_PREAMBLE_BYTE_1) {
 322                 frskyOsdResetReceiveBuffer();
 323                 break;
 324             }
 325             state.recvBuffer.state = RECV_STATE_LENGTH;
 326             break;
 327         case RECV_STATE_LENGTH:
 328             state.recvBuffer.crc = frskyOsdChecksum(state.recvBuffer.crc, c);
 329             state.recvBuffer.expected |= (c & 0x7F) << state.recvBuffer.expectedShift;
 330             state.recvBuffer.expectedShift += 7;
 331             if (c < 0x80) {
 332                 // Full uvarint decoded. Check against buffer size.
 333                 if (state.recvBuffer.expected > sizeof(state.recvBuffer.data)) {
 334                     FRSKY_OSD_ERROR("Can't handle payload of size %u with a buffer of size %u",
 335                         state.recvBuffer.expected, sizeof(state.recvBuffer.data));
 336                     frskyOsdResetReceiveBuffer();
 337                     break;
 338                 }
 339                 FRSKY_OSD_TRACE("Payload of size %u", state.recvBuffer.expected);
 340                 state.recvBuffer.state = state.recvBuffer.expected > 0 ? RECV_STATE_DATA : RECV_STATE_CHECKSUM;
 341             }
 342             break;
 343         case RECV_STATE_DATA:
 344             state.recvBuffer.data[state.recvBuffer.pos++] = c;
 345             state.recvBuffer.crc = frskyOsdChecksum(state.recvBuffer.crc, c);
 346             if (state.recvBuffer.pos == state.recvBuffer.expected) {
 347                 state.recvBuffer.state = RECV_STATE_CHECKSUM;
 348             }
 349             break;
 350         case RECV_STATE_CHECKSUM:
 351             if (c != state.recvBuffer.crc) {
 352                 FRSKY_OSD_DEBUG("Checksum error %u != %u. Discarding %u bytes",
 353                     c, state.recvBuffer.crc, state.recvBuffer.pos);
 354                 frskyOsdResetReceiveBuffer();
 355                 break;
 356             }
 357             state.recvBuffer.state = RECV_STATE_DONE;
 358             break;
 359         case RECV_STATE_DONE:
 360             FRSKY_OSD_DEBUG("Received unexpected byte %u after data", c);
 361             break;
 362         }
 363     }
 364 }
 365
 366 static bool frskyOsdIsResponseAvailable(void)
 367 {
 368     return state.recvBuffer.state == RECV_STATE_DONE;
 369 }
 370
 371 static bool frskyOsdHandleCommand(osdCommand_e cmd, const void *payload, size_t size)
 372 {
 373     switch (cmd) {
 374         case OSD_CMD_RESPONSE_ERROR:
 375         {
 376             if (size >= 2) {
 377                 FRSKY_OSD_ERROR("Received an error %02x in response to command %u", *(ptr + 1), *ptr);
 378                 return true;
 379             }
 380             break;
 381         }
 382         case OSD_CMD_INFO:
 383         {
 384             if (size < sizeof(frskyOsdInfoResponse_t)) {
 385                 break;
 386             }
 387             const frskyOsdInfoResponse_t *resp = payload;
 388             if (resp->magic[0] != 'A' || resp->magic[1] != 'G' || resp->magic[2] != 'H') {
 389                 FRSKY_OSD_ERROR("Invalid magic number %x %x %x, expecting AGH",
 390                     resp->magic[0], resp->magic[1], resp->magic[2]);
 391                 return false;
 392             }
 393             state.info.major = resp->versionMajor;
 394             state.info.minor = resp->versionMinor;
 395             state.info.grid.rows = resp->gridRows;
 396             state.info.grid.columns = resp->gridColumns;
 397             state.info.viewport.width = resp->pixelWidth;
 398             state.info.viewport.height = resp->pixelHeight;
 399             if (!state.initialized) {
 400                 FRSKY_OSD_DEBUG("FrSky OSD initialized. Version %u.%u.%u, pixels=%ux%u, grid=%ux%u",
 401                     resp->versionMajor, resp->versionMinor, resp->versionPatch,
 402                     resp->pixelWidth, resp->pixelHeight, resp->gridColumns, resp->gridRows);
 403                 state.initialized = true;
 404                 frskyOsdClearScreen();
 405                 frskyOsdResetDrawingState();
 406             }
 407             return true;
 408         }
 409         case OSD_CMD_READ_FONT:
 410         {
 411             if (!state.recvOsdCharacter.chr) {
 412                 FRSKY_OSD_DEBUG("Got unexpected font character");
 413                 break;
 414             }
 415             if (size < sizeof(uint16_t) + FRSKY_OSD_CHAR_TOTAL_BYTES) {
 416                 FRSKY_OSD_TRACE("Received buffer too small for a character: %u bytes", size);
 417                 break;
 418             }
 419             const frskyOsdCharacter_t *chr = payload;
 420             state.recvOsdCharacter.addr = chr->addr;
 421             FRSKY_OSD_TRACE("Received character %u", chr->addr);
 422             // Skip character address
 423             memcpy(state.recvOsdCharacter.chr->data, &chr->data, MIN(sizeof(state.recvOsdCharacter.chr->data), (size_t)FRSKY_OSD_CHAR_TOTAL_BYTES));
 424             return true;
 425         }
 426         case OSD_CMD_WRITE_FONT:
 427         {
 428             // We only wait for the confirmation, we're not interested in the data
 429             return true;
 430         }
 431         default:
 432             break;
 433     }
 434     return false;
 435 }
 436
 437 static bool frskyOsdDispatchResponse(void)
 438 {
 439     const uint8_t *payload = state.recvBuffer.data;
 440     int remaining = (int)state.recvBuffer.pos;
 441     bool ok = false;
 442     if (remaining > 0) {
 443         // OSD sends commands one by one, so we don't need to handle
 444         // a frame with multiple ones.
 445         uint8_t cmd = *payload;
 446         payload++;
 447         remaining--;
 448         if (frskyOsdHandleCommand(cmd, payload, remaining)) {
 449             ok = true;
 450         } else {
 451             FRSKY_OSD_DEBUG("Discarding buffer due to unhandled command %u (%d bytes remaining)", cmd, remaining);
 452         }
 453     }
 454     frskyOsdResetReceiveBuffer();
 455     return ok;
 456 }
 457
 458 static void frskyOsdClearReceiveBuffer(void)
 459 {
 460     frskyOsdUpdateReceiveBuffer();
 461
 462     if (frskyOsdIsResponseAvailable()) {
 463         frskyOsdDispatchResponse();
 464     } else if (state.recvBuffer.pos > 0) {
 465         FRSKY_OSD_DEBUG("Discarding receive buffer with %u bytes", state.recvBuffer.pos);
 466         frskyOsdResetReceiveBuffer();
 467     }
 468 }
 469
 470 static void frskyOsdSendAsyncCommand(uint8_t cmd, const void *data, size_t size)
 471 {
 472     FRSKY_OSD_TRACE("Send async cmd %u", cmd);
 473     frskyOsdSendCommand(cmd, data, size);
 474 }
 475
 476 static bool frskyOsdSendSyncCommand(uint8_t cmd, const void *data, size_t size, timeMs_t timeout)
 477 {
 478     FRSKY_OSD_TRACE("Send sync cmd %u", cmd);
 479     frskyOsdClearReceiveBuffer();
 480     frskyOsdSendCommand(cmd, data, size);
 481     frskyOsdFlushSendBuffer();
 482     timeMs_t end = millis() + timeout;
 483     while (millis() < end) {
 484         frskyOsdUpdateReceiveBuffer();
 485         if (frskyOsdIsResponseAvailable() && frskyOsdDispatchResponse()) {
 486             FRSKY_OSD_DEBUG("Got sync response");
 487             return true;
 488         }
 489     }
 490     FRSKY_OSD_DEBUG("Sync response failed");
 491     return false;
 492 }
 493
 494 static bool frskyOsdShouldRequestInfo(void)
 495 {
 496     return !frskyOsdIsReady() || millis() > state.nextInfoRequest;
 497 }
 498
 499 static void frskyOsdRequestInfo(void)
 500 {
 501     timeMs_t now = millis();
 502     if (state.info.nextRequest < now) {
 503         uint8_t version = FRSKY_OSD_SUPPORTED_API_VERSION;
 504         frskyOsdSendAsyncCommand(OSD_CMD_INFO, &version, sizeof(version));
 505         frskyOsdFlushSendBuffer();
 506         state.info.nextRequest = now + FRSKY_OSD_INFO_INTERVAL_MS;
 507     }
 508 }
 509
 510 bool frskyOsdInit(videoSystem_e videoSystem)
 511 {
 512     UNUSED(videoSystem);
 513     FRSKY_OSD_TRACE("frskyOsdInit()");
 514     // TODO: Use videoSystem to set the signal standard when
 515     // no input is detected.
 516     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_FRSKY_OSD);
 517     if (portConfig) {
 518         FRSKY_OSD_TRACE("FrSky OSD configured, trying to connect...");
 519         portOptions_e portOptions = 0;
 520         serialPort_t *port = openSerialPort(portConfig->identifier,
 521             FUNCTION_FRSKY_OSD, NULL, NULL, FRSKY_OSD_BAUDRATE,
 522             MODE_RXTX, portOptions);
 523
 524         if (port) {
 525             frskyOsdStateReset(port);
 526             frskyOsdRequestInfo();
 527             return true;
 528         }
 529     }
 530     return false;
 531 }
 532
 533 bool frskyOsdIsReady(void)
 534 {
 535     return state.info.minor > 0 || state.info.major > 0;
 536 }
 537
 538 void frskyOsdUpdate(void)
 539 {
 540     if (!state.port) {
 541         return;
 542     }
 543     frskyOsdUpdateReceiveBuffer();
 544
 545     if (frskyOsdIsResponseAvailable()) {
 546         frskyOsdDispatchResponse();
 547     }
 548
 549     if (frskyOsdShouldRequestInfo()) {
 550         frskyOsdRequestInfo();
 551     }
 552 }
 553
 554 void frskyOsdBeginTransaction(frskyOsdTransactionOptions_e opts)
 555 {
 556     if (opts & FRSKY_OSD_TRANSACTION_OPT_PROFILED) {
 557         frskyOsdPoint_t p = { .x = 0, .y = 10};
 558         frskyOsdSendAsyncCommand(OSD_CMD_TRANSACTION_BEGIN_PROFILED, &p, sizeof(p));
 559         if (opts & FRSKY_OSD_TRANSACTION_OPT_RESET_DRAWING) {
 560             frskyOsdResetDrawingState();
 561         }
 562     } else if (opts & FRSKY_OSD_TRANSACTION_OPT_RESET_DRAWING) {
 563         frskyOsdSendAsyncCommand(OSD_CMD_TRANSACTION_BEGIN_RESET_DRAWING, NULL, 0);
 564     } else {
 565         frskyOsdSendAsyncCommand(OSD_CMD_TRANSACTION_BEGIN, NULL, 0);
 566     }
 567 }
 568
 569 void frskyOsdCommitTransaction(void)
 570 {
 571     // Check wether the only command in the queue is a transaction begin.
 572     // In that, case, discard the send buffer since it will make generate
 573     // an empty transaction.
 574     if (state.sendBuffer.pos == 1) {
 575         if (state.sendBuffer.data[0] == OSD_CMD_TRANSACTION_BEGIN ||
 576             state.sendBuffer.data[0] == OSD_CMD_TRANSACTION_BEGIN_RESET_DRAWING) {
 577
 578             state.sendBuffer.pos = 0;
 579             return;
 580         }
 581     }
 582     frskyOsdSendAsyncCommand(OSD_CMD_TRANSACTION_COMMIT, NULL, 0);
 583     frskyOsdFlushSendBuffer();
 584 }
 585
 586 void frskyOsdFlushSendBuffer(void)
 587 {
 588     if (state.sendBuffer.pos > 0) {
 589         frskyOsdProcessCommandU8(NULL, FRSKY_OSD_PREAMBLE_BYTE_0);
 590         frskyOsdProcessCommandU8(NULL, FRSKY_OSD_PREAMBLE_BYTE_1);
 591
 592         uint8_t crc = 0;
 593         uint8_t buffer[4];
 594         int lengthSize = uvarintEncode(state.sendBuffer.pos, buffer, sizeof(buffer));
 595         for (int ii = 0; ii < lengthSize; ii++) {
 596             frskyOsdProcessCommandU8(&crc, buffer[ii]);
 597         }
 598         for (unsigned ii = 0; ii < state.sendBuffer.pos; ii++) {
 599             frskyOsdProcessCommandU8(&crc, state.sendBuffer.data[ii]);
 600         }
 601         frskyOsdProcessCommandU8(NULL, crc);
 602         state.sendBuffer.pos = 0;
 603     }
 604 }
 605
 606 bool frskyOsdReadFontCharacter(unsigned char_address, osdCharacter_t *chr)
 607 {
 608     uint16_t addr = char_address;
 609
 610     state.recvOsdCharacter.addr = UINT16_MAX;
 611     state.recvOsdCharacter.chr = chr;
 612
 613     // 500ms should be more than enough to receive ~70 bytes @ 115200 bps
 614     bool ok = frskyOsdSendSyncCommand(OSD_CMD_READ_FONT, &addr, sizeof(addr), 500);
 615
 616     state.recvOsdCharacter.chr = NULL;
 617
 618     if (ok && state.recvOsdCharacter.addr == addr) {
 619         return true;
 620     }
 621     return false;
 622 }
 623
 624 bool frskyOsdWriteFontCharacter(unsigned char_address, const osdCharacter_t *chr)
 625 {
 626     frskyOsdCharacter_t c;
 627     STATIC_ASSERT(sizeof(*chr) == sizeof(c.data), invalid_character_size);
 628
 629     memcpy(&c.data, chr, sizeof(c.data));
 630     c.addr = char_address;
 631     FRSKY_OSD_TRACE("Writing font character %u", char_address);
 632     frskyOsdSendSyncCommand(OSD_CMD_WRITE_FONT, &c, sizeof(c), 1000);
 633     return true;
 634 }
 635
 636 unsigned frskyOsdGetGridRows(void)
 637 {
 638     return state.info.grid.rows;
 639 }
 640
 641 unsigned frskyOsdGetGridCols(void)
 642 {
 643     return state.info.grid.columns;
 644 }
 645
 646 unsigned frskyOsdGetPixelWidth(void)
 647 {
 648     return state.info.viewport.width;
 649 }
 650
 651 unsigned frskyOsdGetPixelHeight(void)
 652 {
 653     return state.info.viewport.height;
 654 }
 655
 656 static void frskyOsdSendCharInGrid(unsigned x, unsigned y, uint16_t chr)
 657 {
 658     uint8_t payload[] = {
 659         x,
 660         y,
 661         chr & 0xFF,
 662         chr >> 8,
 663         0,
 664     };
 665     frskyOsdSendAsyncCommand(OSD_CMD_DRAW_GRID_CHR, payload, sizeof(payload));
 666 }
 667
 668 static void frskyOsdSendAsyncBlobCommand(uint8_t cmd, const void *header, size_t headerSize, const void *blob, size_t blobSize)
 669 {
 670     uint8_t payload[128];
 671
 672     memcpy(payload, header, headerSize);
 673
 674     int uvarintSize = uvarintEncode(blobSize, &payload[headerSize], sizeof(payload) - headerSize);
 675     memcpy(&payload[headerSize + uvarintSize], blob, blobSize);
 676     frskyOsdSendAsyncCommand(cmd, payload,  headerSize + uvarintSize + blobSize);
 677 }
 678
 679 void frskyOsdDrawStringInGrid(unsigned x, unsigned y, const char *buff)
 680 {
 681     frskyOsdDrawGridStrHeaderCmd_t cmd;
 682     cmd.gx = x;
 683     cmd.gy = y;
 684     cmd.opts = 0;
 685
 686     frskyOsdSendAsyncBlobCommand(OSD_CMD_DRAW_GRID_STR, &cmd, sizeof(cmd), buff, strlen(buff) + 1);
 687 }
 688
 689 void frskyOsdDrawCharInGrid(unsigned x, unsigned y, uint16_t chr)
 690 {
 691     frskyOsdSendCharInGrid(x, y, chr);
 692 }
 693
 694 void frskyOsdClearScreen(void)
 695 {
 696     if (!frskyOsdIsReady()) {
 697         return;
 698     }
 699     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_CLEAR_SCREEN, NULL, 0);
 700 }
 701
 702 void frskyOsdSetStrokeColor(frskyOsdColor_e color)
 703 {
 704     uint8_t c = color;
 705     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_SET_STROKE_COLOR, &c, sizeof(c));
 706 }
 707
 708 void frskyOsdSetFillColor(frskyOsdColor_e color)
 709 {
 710     uint8_t c = color;
 711     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_SET_FILL_COLOR, &c, sizeof(c));
 712 }
 713
 714 void frskyOsdSetStrokeAndFillColor(frskyOsdColor_e color)
 715 {
 716     uint8_t c = color;
 717     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_SET_STROKE_AND_FILL_COLOR, &c, sizeof(c));
 718 }
 719
 720 void frskyOsdSetColorInversion(bool inverted)
 721 {
 722     uint8_t c = inverted ? 1 : 0;
 723     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_SET_COLOR_INVERSION, &c, sizeof(c));
 724 }
 725
 726 void frskyOsdSetPixel(int x, int y, frskyOsdColor_e color)
 727 {
 728     frskyOsdSetPixel_t sp = {.p = {.x = x, .y = y}, .color = color};
 729     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_SET_PIXEL, &sp, sizeof(sp));
 730 }
 731
 732 void frskyOsdSetPixelToStrokeColor(int x, int y)
 733 {
 734     frskyOsdPoint_t p = { .x = x, .y = y};
 735     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_SET_PIXEL_TO_STROKE_COLOR, &p, sizeof(p));
 736 }
 737
 738 void frskyOsdSetPixelToFillColor(int x, int y)
 739 {
 740     frskyOsdPoint_t p = { .x = x, .y = y};
 741     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_SET_PIXEL_TO_FILL_COLOR, &p, sizeof(p));
 742 }
 743
 744 void frskyOsdSetStrokeWidth(unsigned width)
 745 {
 746     uint8_t w = width;
 747     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_SET_STROKE_WIDTH, &w, sizeof(w));
 748 }
 749
 750 void frskyOsdSetLineOutlineType(frskyOsdLineOutlineType_e outlineType)
 751 {
 752     uint8_t type = outlineType;
 753     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_SET_LINE_OUTLINE_TYPE, &type, sizeof(type));
 754 }
 755
 756 void frskyOsdSetLineOutlineColor(frskyOsdColor_e outlineColor)
 757 {
 758     uint8_t color = outlineColor;
 759     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_SET_LINE_OUTLINE_COLOR, &color, sizeof(color));
 760 }
 761
 762 void frskyOsdClipToRect(int x, int y, int w, int h)
 763 {
 764     frskyOsdRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
 765     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_CLIP_TO_RECT, &r, sizeof(r));
 766 }
 767
 768 void frskyOsdClearRect(int x, int y, int w, int h)
 769 {
 770     frskyOsdRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
 771     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_CLEAR_RECT, &r, sizeof(r));
 772 }
 773
 774 void frskyOsdResetDrawingState(void)
 775 {
 776     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_RESET, NULL, 0);
 777 }
 778
 779 void frskyOsdDrawCharacter(int x, int y, uint16_t chr, uint8_t opts)
 780 {
 781     frskyOsdDrawCharacterCmd_t dc = { .p = {.x = x, .y = y}, .chr = chr, .opts = opts};
 782     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_DRAW_CHAR, &dc, sizeof(dc));
 783 }
 784
 785 void frskyOsdDrawCharacterMask(int x, int y, uint16_t chr, frskyOsdColor_e color, uint8_t opts)
 786 {
 787     frskyOsdDrawCharacterMaskCmd_t dc = { .dc = { .p = {.x = x, .y = y}, .chr = chr, .opts = opts}, .maskColor = color};
 788     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_DRAW_CHAR_MASK, &dc, sizeof(dc));
 789 }
 790
 791 void frskyOsdDrawString(int x, int y, const char *s, uint8_t opts)
 792 {
 793     frskyOsdDrawStrCommandHeaderCmd_t cmd;
 794     cmd.p.x = x;
 795     cmd.p.y = y;
 796     cmd.opts = opts;
 797
 798     frskyOsdSendAsyncBlobCommand(OSD_CMD_DRAWING_DRAW_STRING, &cmd, sizeof(cmd), s, strlen(s) + 1);
 799 }
 800
 801 void frskyOsdDrawStringMask(int x, int y, const char *s, frskyOsdColor_e color, uint8_t opts)
 802 {
 803     frskyOsdDrawStrMaskCommandHeaderCmd_t cmd;
 804     cmd.p.x = x;
 805     cmd.p.y = y;
 806     cmd.opts = opts;
 807     cmd.maskColor = color;
 808
 809     frskyOsdSendAsyncBlobCommand(OSD_CMD_DRAWING_DRAW_STRING_MASK, &cmd, sizeof(cmd), s, strlen(s) + 1);
 810 }
 811
 812 void frskyOsdMoveToPoint(int x, int y)
 813 {
 814     frskyOsdPoint_t p = { .x = x, .y = y};
 815     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_MOVE_TO_POINT, &p, sizeof(p));
 816 }
 817
 818 void frskyOsdStrokeLineToPoint(int x, int y)
 819 {
 820     frskyOsdPoint_t p = { .x = x, .y = y};
 821     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_STROKE_LINE_TO_POINT, &p, sizeof(p));
 822 }
 823
 824 void frskyOsdStrokeTriangle(int x1, int y1, int x2, int y2, int x3, int y3)
 825 {
 826     frskyOsdTriangle_t t = {.p1 = {.x = x1, .y = y1}, .p2 = {.x = x2, .y = y2}, .p3 = { .x = x3, .y = y3}};
 827     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_STROKE_TRIANGLE, &t, sizeof(t));
 828 }
 829
 830 void frskyOsdFillTriangle(int x1, int y1, int x2, int y2, int x3, int y3)
 831 {
 832     frskyOsdTriangle_t t = {.p1 = {.x = x1, .y = y1}, .p2 = {.x = x2, .y = y2}, .p3 = { .x = x3, .y = y3}};
 833     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_FILL_TRIANGLE, &t, sizeof(t));
 834 }
 835
 836 void frskyOsdFillStrokeTriangle(int x1, int y1, int x2, int y2, int x3, int y3)
 837 {
 838     frskyOsdTriangle_t t = {.p1 = {.x = x1, .y = y1}, .p2 = {.x = x2, .y = y2}, .p3 = { .x = x3, .y = y3}};
 839     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_FILL_STROKE_TRIANGLE, &t, sizeof(t));
 840 }
 841
 842 void frskyOsdStrokeRect(int x, int y, int w, int h)
 843 {
 844     frskyOsdRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
 845     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_STROKE_RECT, &r, sizeof(r));
 846 }
 847
 848 void frskyOsdFillRect(int x, int y, int w, int h)
 849 {
 850     frskyOsdRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
 851     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_FILL_RECT, &r, sizeof(r));
 852 }
 853
 854 void frskyOsdFillStrokeRect(int x, int y, int w, int h)
 855 {
 856     frskyOsdRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
 857     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_FILL_STROKE_RECT, &r, sizeof(r));
 858 }
 859
 860 void frskyOsdStrokeEllipseInRect(int x, int y, int w, int h)
 861 {
 862     frskyOsdRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
 863     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_STROKE_ELLIPSE_IN_RECT, &r, sizeof(r));
 864 }
 865
 866 void frskyOsdFillEllipseInRect(int x, int y, int w, int h)
 867 {
 868     frskyOsdRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
 869     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_FILL_ELLIPSE_IN_RECT, &r, sizeof(r));
 870 }
 871
 872 void frskyOsdFillStrokeEllipseInRect(int x, int y, int w, int h)
 873 {
 874     frskyOsdRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
 875     frskyOsdSendAsyncCommand(OSD_CMD_DRAWING_FILL_STROKE_ELLIPSE_IN_RECT, &r, sizeof(r));
 876 }
 877
 878 void frskyOsdCtmReset(void)
 879 {
 880     frskyOsdSendAsyncCommand(OSD_CMD_CTM_RESET, NULL, 0);
 881 }
 882
 883 void frskyOsdCtmSet(float m11, float m12, float m21, float m22, float m31, float m32)
 884 {
 885     float values[] = {
 886         m11, m12,
 887         m21, m22,
 888         m31, m32,
 889     };
 890     frskyOsdSendAsyncCommand(OSD_CMD_CTM_SET, values, sizeof(values));
 891 }
 892
 893 void frskyOsdCtmTranslate(float tx, float ty)
 894 {
 895     float values[] = {
 896         tx,
 897         ty,
 898     };
 899     frskyOsdSendAsyncCommand(OSD_CMD_CTM_TRANSLATE, values, sizeof(values));
 900 }
 901
 902 void frskyOsdCtmScale(float sx, float sy)
 903 {
 904     float values[] = {
 905         sx,
 906         sy,
 907     };
 908     frskyOsdSendAsyncCommand(OSD_CMD_CTM_SCALE, values, sizeof(values));
 909 }
 910
 911 void frskyOsdCtmRotate(float r)
 912 {
 913     frskyOsdSendAsyncCommand(OSD_CMD_CTM_ROTATE, &r, sizeof(r));
 914 }
 915
 916 void frskyOsdContextPush(void)
 917 {
 918     frskyOsdSendAsyncCommand(OSD_CMD_CONTEXT_PUSH, NULL, 0);
 919 }
 920
 921 void frskyOsdContextPop(void)
 922 {
 923     frskyOsdSendAsyncCommand(OSD_CMD_CONTEXT_POP, NULL, 0);
 924 }
 925
 926
 927 #endif