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_OSD) && defined(USE_FRSKYOSD)
   8
   9 #include "common/crc.h"
  10 #include "common/log.h"
  11 #include "common/maths.h"
  12 #include "common/time.h"
  13 #include "common/utils.h"
  14 #include "common/uvarint.h"
  15
  16 #include "drivers/time.h"
  17
  18 #include "io/frsky_osd.h"
  19 #include "io/serial.h"
  20
  21 #define FRSKY_OSD_DEFAULT_BAUDRATE_INDEX BAUD_115200
  22 #define FRSKY_OSD_SUPPORTED_API_VERSION 2
  23
  24 #define FRSKY_OSD_PREAMBLE_BYTE_0 '$'
  25 #define FRSKY_OSD_PREAMBLE_BYTE_1 'A'
  26
  27 #define FRSKY_OSD_GRID_BUFFER_CHAR_BITS 9
  28 #define FRSKY_OSD_GRID_BUFFER_CHAR_MASK ((1 << FRSKY_OSD_GRID_BUFFER_CHAR_BITS) - 1)
  29 #define FRSKY_OSD_GRID_BUFFER_ENCODE(chr, attr) ((chr & FRSKY_OSD_GRID_BUFFER_CHAR_MASK) | (attr << FRSKY_OSD_GRID_BUFFER_CHAR_BITS))
  30
  31 #define FRSKY_OSD_CHAR_ATTRIBUTE_COLOR_INVERSE (1 << 0)
  32 #define FRSKY_OSD_CHAR_ATTRIBUTE_SOLID_BACKGROUND (1 << 1)
  33
  34 #define FRSKY_OSD_CHAR_DATA_BYTES 54
  35 #define FRSKY_OSD_CHAR_METADATA_BYTES 10
  36 #define FRSKY_OSD_CHAR_TOTAL_BYTES (FRSKY_OSD_CHAR_DATA_BYTES + FRSKY_OSD_CHAR_METADATA_BYTES)
  37
  38 #define FRSKY_OSD_SEND_BUFFER_SIZE 192
  39 #define FRSKY_OSD_RECV_BUFFER_SIZE 128
  40
  41 #define FRSKY_OSD_CMD_RESPONSE_ERROR 0
  42
  43 #define FRSKY_OSD_INFO_INTERVAL_MS 100
  44 #define FRSKY_OSD_INFO_READY_INTERVAL_MS 5000
  45
  46 #define FRSKY_OSD_TRACE(fmt, ...)
  47 #define FRSKY_OSD_DEBUG(fmt, ...) LOG_DEBUG(OSD, "FrSky OSD: " fmt,  ##__VA_ARGS__)
  48 #define FRSKY_OSD_ERROR(fmt, ...) LOG_ERROR(OSD, "FrSky OSD: " fmt,  ##__VA_ARGS__)
  49 #define FRSKY_OSD_ASSERT(x)
  50
  51 typedef enum
  52 {
  53     OSD_CMD_RESPONSE_ERROR = 0,
  54
  55     OSD_CMD_INFO = 1,
  56     OSD_CMD_READ_FONT = 2,
  57     OSD_CMD_WRITE_FONT = 3,
  58     OSD_CMD_GET_CAMERA = 4,
  59     OSD_CMD_SET_CAMERA = 5,
  60     OSD_CMD_GET_ACTIVE_CAMERA = 6,
  61     OSD_CMD_GET_OSD_ENABLED = 7,
  62     OSD_CMD_SET_OSD_ENABLED = 8,
  63
  64     OSD_CMD_TRANSACTION_BEGIN = 16,
  65     OSD_CMD_TRANSACTION_COMMIT = 17,
  66     OSD_CMD_TRANSACTION_BEGIN_PROFILED = 18,
  67     OSD_CMD_TRANSACTION_BEGIN_RESET_DRAWING = 19,
  68
  69     OSD_CMD_DRAWING_SET_STROKE_COLOR = 22,
  70     OSD_CMD_DRAWING_SET_FILL_COLOR = 23,
  71     OSD_CMD_DRAWING_SET_STROKE_AND_FILL_COLOR = 24,
  72     OSD_CMD_DRAWING_SET_COLOR_INVERSION = 25,
  73     OSD_CMD_DRAWING_SET_PIXEL = 26,
  74     OSD_CMD_DRAWING_SET_PIXEL_TO_STROKE_COLOR = 27,
  75     OSD_CMD_DRAWING_SET_PIXEL_TO_FILL_COLOR = 28,
  76     OSD_CMD_DRAWING_SET_STROKE_WIDTH = 29,
  77     OSD_CMD_DRAWING_SET_LINE_OUTLINE_TYPE = 30,
  78     OSD_CMD_DRAWING_SET_LINE_OUTLINE_COLOR = 31,
  79
  80     OSD_CMD_DRAWING_CLIP_TO_RECT = 40,
  81     OSD_CMD_DRAWING_CLEAR_SCREEN = 41,
  82     OSD_CMD_DRAWING_CLEAR_RECT = 42,
  83     OSD_CMD_DRAWING_RESET = 43,
  84     OSD_CMD_DRAWING_DRAW_BITMAP = 44,
  85     OSD_CMD_DRAWING_DRAW_BITMAP_MASK = 45,
  86     OSD_CMD_DRAWING_DRAW_CHAR = 46,
  87     OSD_CMD_DRAWING_DRAW_CHAR_MASK = 47,
  88     OSD_CMD_DRAWING_DRAW_STRING = 48,
  89     OSD_CMD_DRAWING_DRAW_STRING_MASK = 49,
  90     OSD_CMD_DRAWING_MOVE_TO_POINT = 50,
  91     OSD_CMD_DRAWING_STROKE_LINE_TO_POINT = 51,
  92     OSD_CMD_DRAWING_STROKE_TRIANGLE = 52,
  93     OSD_CMD_DRAWING_FILL_TRIANGLE = 53,
  94     OSD_CMD_DRAWING_FILL_STROKE_TRIANGLE = 54,
  95     OSD_CMD_DRAWING_STROKE_RECT = 55,
  96     OSD_CMD_DRAWING_FILL_RECT = 56,
  97     OSD_CMD_DRAWING_FILL_STROKE_RECT = 57,
  98     OSD_CMD_DRAWING_STROKE_ELLIPSE_IN_RECT = 58,
  99     OSD_CMD_DRAWING_FILL_ELLIPSE_IN_RECT = 59,
 100     OSD_CMD_DRAWING_FILL_STROKE_ELLIPSE_IN_RECT = 60,
 101
 102     OSD_CMD_CTM_RESET = 80,
 103     OSD_CMD_CTM_SET = 81,
 104     OSD_CMD_CTM_TRANSLATE = 82,
 105     OSD_CMD_CTM_SCALE = 83,
 106     OSD_CMD_CTM_ROTATE = 84,
 107     OSD_CMD_CTM_ROTATE_ABOUT = 85,
 108     OSD_CMD_CTM_SHEAR = 86,
 109     OSD_CMD_CTM_SHEAR_ABOUT = 87,
 110     OSD_CMD_CTM_MULTIPLY = 88,
 111
 112     OSD_CMD_CONTEXT_PUSH = 100,
 113     OSD_CMD_CONTEXT_POP = 101,
 114
 115     // MAX7456 emulation commands
 116     OSD_CMD_DRAW_GRID_CHR = 110,
 117     OSD_CMD_DRAW_GRID_STR = 111,
 118     OSD_CMD_DRAW_GRID_CHR_2 = 112,                                  // API2
 119     OSD_CMD_DRAW_GRID_STR_2 = 113,                                  // API2
 120
 121     OSD_CMD_WIDGET_SET_CONFIG = 115,                                // API2
 122     OSD_CMD_WIDGET_DRAW = 116,                                      // API2
 123     OSD_CMD_WIDGET_ERASE = 117,                                     // API2
 124
 125     OSD_CMD_SET_DATA_RATE = 122,
 126 } osdCommand_e;
 127
 128 typedef enum {
 129     RECV_STATE_NONE,
 130     RECV_STATE_SYNC,
 131     RECV_STATE_LENGTH,
 132     RECV_STATE_DATA,
 133     RECV_STATE_CHECKSUM,
 134     RECV_STATE_DONE,
 135 } frskyOSDRecvState_e;
 136
 137 typedef struct frskyOSDInfoResponse_s {
 138     uint8_t magic[3];
 139     uint8_t versionMajor;
 140     uint8_t versionMinor;
 141     uint8_t versionPatch;
 142     uint8_t gridRows;
 143     uint8_t gridColumns;
 144     uint16_t pixelWidth;
 145     uint16_t pixelHeight;
 146     uint8_t tvStandard;
 147     uint8_t hasDetectedCamera;
 148     uint16_t maxFrameSize;
 149     uint8_t contextStackSize;
 150 } __attribute__((packed)) frskyOSDInfoResponse_t;
 151
 152 typedef struct frskyOSDFontCharacter_s {
 153     uint16_t addr;
 154     struct {
 155         uint8_t bitmap[FRSKY_OSD_CHAR_DATA_BYTES]; // 12x18 2bpp
 156         uint8_t metadata[FRSKY_OSD_CHAR_METADATA_BYTES];
 157     } data;
 158 } __attribute__((packed)) frskyOSDCharacter_t;
 159
 160 typedef struct frskyOSDDrawGridCharCmd_s {
 161     uint8_t gx;
 162     uint8_t gy;
 163     uint16_t chr;
 164     uint8_t opts;
 165 } __attribute__((packed)) frskyOSDDrawGridCharCmd_t;
 166
 167 typedef struct frskyOSDDrawGridStrHeaderCmd_s {
 168     uint8_t gx;
 169     uint8_t gy;
 170     uint8_t opts;
 171     // uvarint with size and blob follow
 172     // string IS null-terminated
 173 } __attribute__((packed)) frskyOSDDrawGridStrHeaderCmd_t;
 174
 175 typedef struct frskyOSDDrawGridStrV2HeaderCmd_s {
 176     unsigned gx : 5;   // +5 = 5
 177     unsigned gy : 4;   // +4 = 9
 178     unsigned opts : 3; // +3 = 12
 179     unsigned size : 4; // +4 = 16 = 2 bytes
 180     // if size == 0, uvarint with size follows
 181     // blob with the given size follows
 182     // string IS NOT null terminated
 183 } __attribute__((packed)) frskyOSDDrawGridStrV2HeaderCmd_t;
 184
 185 typedef struct frskyOSDTriangle_s {
 186     frskyOSDPoint_t p1;
 187     frskyOSDPoint_t p2;
 188     frskyOSDPoint_t p3;
 189 } __attribute__((packed)) frskyOSDTriangle_t;
 190
 191 typedef struct frskyOSDSetPixel_s {
 192     frskyOSDPoint_t p;
 193     uint8_t color;
 194 }  __attribute__((packed)) frskyOSDSetPixel_t;
 195
 196 typedef struct frskyOSDDrawCharacterCmd_s {
 197     frskyOSDPoint_t p;
 198     uint16_t chr;
 199     uint8_t opts;
 200 }  __attribute__((packed)) frskyOSDDrawCharacterCmd_t;
 201
 202 typedef struct frskyOSDDrawCharacterMaskCmd_s {
 203     frskyOSDDrawCharacterCmd_t dc;
 204     uint8_t maskColor;
 205 }  __attribute__((packed)) frskyOSDDrawCharacterMaskCmd_t;
 206
 207 typedef struct frskyOSDDrawStrCommandHeaderCmd_s {
 208     frskyOSDPoint_t p;
 209     uint8_t opts;
 210     // uvarint with size and blob follow
 211 } __attribute__((packed)) frskyOSDDrawStrCommandHeaderCmd_t;
 212
 213 typedef struct frskyOSDDrawStrMaskCommandHeaderCmd_s {
 214     frskyOSDPoint_t p;
 215     uint8_t opts;
 216     uint8_t maskColor;
 217     // uvarint with size and blob follow
 218 } __attribute__((packed)) frskyOSDDrawStrMaskCommandHeaderCmd_t;
 219
 220 typedef struct frskyOSDDrawGridChrV2Cmd_s
 221 {
 222     unsigned gx : 5;        // +5 = 5
 223     unsigned gy : 4;        // +4 = 9
 224     unsigned chr : 9;       // +9 = 18
 225     unsigned opts : 3;      // +3 = 21, from osd_bitmap_opt_t
 226     unsigned as_mask : 1;   // +1 = 22
 227     unsigned color : 2;     // +2 = 24 = 3 bytes, only used when drawn as as_mask = 1
 228 } __attribute__((packed)) frskyOSDDrawGridChrV2Cmd_t;
 229
 230 typedef struct frskyOSDError_s {
 231     uint8_t command;
 232     int8_t code;
 233 } frskyOSDError_t;
 234
 235
 236 typedef struct frskyOSDState_s {
 237     struct {
 238         uint8_t data[FRSKY_OSD_SEND_BUFFER_SIZE];
 239         uint8_t pos;
 240     } sendBuffer;
 241     struct {
 242         uint8_t state;
 243         uint8_t crc;
 244         uint16_t expected;
 245         uint8_t expectedShift;
 246         uint8_t data[FRSKY_OSD_RECV_BUFFER_SIZE];
 247         uint8_t pos;
 248     } recvBuffer;
 249     struct {
 250         uint8_t major;
 251         uint8_t minor;
 252         timeMs_t nextRequest;
 253         struct {
 254             uint8_t rows;
 255             uint8_t columns;
 256         } grid;
 257         struct {
 258             uint16_t width;
 259             uint16_t height;
 260         } viewport;
 261     } info;
 262     struct {
 263         uint16_t addr;
 264         osdCharacter_t *chr;
 265     } recvOsdCharacter;
 266     serialPort_t *port;
 267     baudRate_e baudrate;
 268     bool keepBaudrate;
 269     bool initialized;
 270     frskyOSDError_t error;
 271     timeMs_t nextInfoRequest;
 272 } frskyOSDState_t;
 273
 274 static frskyOSDState_t state;
 275
 276 static bool frskyOSDDispatchResponse(void);
 277
 278 static uint8_t frskyOSDChecksum(uint8_t crc, uint8_t c)
 279 {
 280     return crc8_dvb_s2(crc, c);
 281 }
 282
 283 static bool frskyOSDSpeaksV2(void)
 284 {
 285     return state.info.major >= 2 || (state.info.major == 1 && state.info.minor >= 99);
 286 }
 287
 288 static void frskyOSDResetReceiveBuffer(void)
 289 {
 290     state.recvBuffer.state = RECV_STATE_NONE;
 291     state.recvBuffer.crc = 0;
 292     state.recvBuffer.expected = 0;
 293     state.recvBuffer.expectedShift = 0;
 294     state.recvBuffer.pos = 0;
 295 }
 296
 297 static void frskyOSDResetSendBuffer(void)
 298 {
 299     state.sendBuffer.pos = 0;
 300 }
 301
 302 static void frskyOSDProcessCommandU8(uint8_t *crc, uint8_t c)
 303 {
 304     while (serialTxBytesFree(state.port) == 0) {
 305     };
 306     serialWrite(state.port, c);
 307     if (crc) {
 308         *crc = crc8_dvb_s2(*crc, c);
 309     }
 310 }
 311
 312 static void frskyOSDSendCommand(uint8_t cmd, const void *payload, size_t size)
 313 {
 314     int required = size + 1;
 315     FRSKY_OSD_ASSERT(required <= sizeof(state.sendBuffer.data));
 316     int rem = sizeof(state.sendBuffer.data) - state.sendBuffer.pos;
 317     if (rem < required) {
 318         frskyOSDFlushSendBuffer();
 319     }
 320     state.sendBuffer.data[state.sendBuffer.pos++] = cmd;
 321     const uint8_t *ptr = payload;
 322     for (size_t ii = 0; ii < size; ii++, ptr++) {
 323         state.sendBuffer.data[state.sendBuffer.pos++] = *ptr;
 324     }
 325 }
 326
 327 static void frskyOSDStateReset(serialPort_t *port, baudRate_e baudrate)
 328 {
 329     frskyOSDResetReceiveBuffer();
 330     frskyOSDResetSendBuffer();
 331     state.info.grid.rows = 0;
 332     state.info.grid.columns = 0;
 333     state.info.viewport.width = 0;
 334     state.info.viewport.height = 0;
 335
 336     state.port = port;
 337     state.baudrate = baudrate;
 338     state.keepBaudrate = false;
 339     state.initialized = false;
 340 }
 341
 342 static void frskyOSDUpdateReceiveBuffer(void)
 343 {
 344     while (serialRxBytesWaiting(state.port) > 0) {
 345         uint8_t c = serialRead(state.port);
 346         switch ((frskyOSDRecvState_e)state.recvBuffer.state) {
 347             case RECV_STATE_NONE:
 348                 if (c != FRSKY_OSD_PREAMBLE_BYTE_0) {
 349                     break;
 350                 }
 351                 state.recvBuffer.state = RECV_STATE_SYNC;
 352                 break;
 353             case RECV_STATE_SYNC:
 354                 if (c != FRSKY_OSD_PREAMBLE_BYTE_1) {
 355                     frskyOSDResetReceiveBuffer();
 356                     break;
 357                 }
 358                 state.recvBuffer.state = RECV_STATE_LENGTH;
 359                 break;
 360             case RECV_STATE_LENGTH:
 361                 state.recvBuffer.crc = frskyOSDChecksum(state.recvBuffer.crc, c);
 362                 state.recvBuffer.expected |= (c & 0x7F) << state.recvBuffer.expectedShift;
 363                 state.recvBuffer.expectedShift += 7;
 364                 if (c < 0x80) {
 365                     // Full uvarint decoded. Check against buffer size.
 366                     if (state.recvBuffer.expected > sizeof(state.recvBuffer.data)) {
 367                         FRSKY_OSD_ERROR("Can't handle payload of size %u with a buffer of size %u",
 368                             state.recvBuffer.expected, (unsigned int)sizeof(state.recvBuffer.data));
 369                         frskyOSDResetReceiveBuffer();
 370                         break;
 371                     }
 372                     FRSKY_OSD_TRACE("Payload of size %u", state.recvBuffer.expected);
 373                     state.recvBuffer.state = state.recvBuffer.expected > 0 ? RECV_STATE_DATA : RECV_STATE_CHECKSUM;
 374                 }
 375                 break;
 376             case RECV_STATE_DATA:
 377                 state.recvBuffer.data[state.recvBuffer.pos++] = c;
 378                 state.recvBuffer.crc = frskyOSDChecksum(state.recvBuffer.crc, c);
 379                 if (state.recvBuffer.pos == state.recvBuffer.expected) {
 380                     state.recvBuffer.state = RECV_STATE_CHECKSUM;
 381                 }
 382                 break;
 383             case RECV_STATE_CHECKSUM:
 384                 if (c != state.recvBuffer.crc) {
 385                     FRSKY_OSD_DEBUG("Checksum error %u != %u. Discarding %u bytes",
 386                         c, state.recvBuffer.crc, state.recvBuffer.pos);
 387                     frskyOSDResetReceiveBuffer();
 388                     break;
 389                 }
 390                 state.recvBuffer.state = RECV_STATE_DONE;
 391                 break;
 392             case RECV_STATE_DONE:
 393                 FRSKY_OSD_DEBUG("Received unexpected byte %u after data", c);
 394                 break;
 395         }
 396     }
 397 }
 398
 399 static bool frskyOSDIsResponseAvailable(void)
 400 {
 401     return state.recvBuffer.state == RECV_STATE_DONE;
 402 }
 403
 404 static void frskyOSDClearReceiveBuffer(void)
 405 {
 406     frskyOSDUpdateReceiveBuffer();
 407
 408     if (frskyOSDIsResponseAvailable()) {
 409         frskyOSDDispatchResponse();
 410     } else if (state.recvBuffer.pos > 0) {
 411         FRSKY_OSD_DEBUG("Discarding receive buffer with %u bytes", state.recvBuffer.pos);
 412         frskyOSDResetReceiveBuffer();
 413     }
 414 }
 415
 416 static void frskyOSDSendAsyncCommand(uint8_t cmd, const void *data, size_t size)
 417 {
 418     FRSKY_OSD_TRACE("Send async cmd %u", cmd);
 419     frskyOSDSendCommand(cmd, data, size);
 420 }
 421
 422 static bool frskyOSDSendSyncCommand(uint8_t cmd, const void *data, size_t size, timeMs_t timeout)
 423 {
 424     FRSKY_OSD_TRACE("Send sync cmd %u", cmd);
 425     frskyOSDClearReceiveBuffer();
 426     frskyOSDSendCommand(cmd, data, size);
 427     frskyOSDFlushSendBuffer();
 428     timeMs_t end = millis() + timeout;
 429     while (millis() < end) {
 430         frskyOSDUpdateReceiveBuffer();
 431         if (frskyOSDIsResponseAvailable() && frskyOSDDispatchResponse()) {
 432             FRSKY_OSD_TRACE("Got sync response");
 433             return true;
 434         }
 435     }
 436     FRSKY_OSD_DEBUG("Sync response failed");
 437     return false;
 438 }
 439
 440 static bool frskyOSDHandleCommand(osdCommand_e cmd, const void *payload, size_t size)
 441 {
 442     const uint8_t *ptr = payload;
 443
 444     state.error.command = 0;
 445     state.error.code = 0;
 446
 447     switch (cmd) {
 448         case OSD_CMD_RESPONSE_ERROR:
 449         {
 450             if (size >= 2) {
 451                 state.error.command = *ptr;
 452                 state.error.code = *(ptr + 1);
 453                 FRSKY_OSD_ERROR("Received an error %02x in response to command %u", *(ptr + 1), *ptr);
 454                 return true;
 455             }
 456             break;
 457         }
 458         case OSD_CMD_INFO:
 459         {
 460             if (size < sizeof(frskyOSDInfoResponse_t)) {
 461                 break;
 462             }
 463             const frskyOSDInfoResponse_t *resp = payload;
 464             if (resp->magic[0] != 'A' || resp->magic[1] != 'G' || resp->magic[2] != 'H') {
 465                 FRSKY_OSD_ERROR("invalid magic number %x %x %x, expecting AGH",
 466                     resp->magic[0], resp->magic[1], resp->magic[2]);
 467                 return false;
 468             }
 469             FRSKY_OSD_TRACE("received OSD_CMD_INFO at %u", (unsigned)baudRates[state.baudrate]);
 470             if (!state.keepBaudrate) {
 471                 const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_FRSKY_OSD);
 472                 if (portConfig && portConfig->peripheral_baudrateIndex > FRSKY_OSD_DEFAULT_BAUDRATE_INDEX &&
 473                     portConfig->peripheral_baudrateIndex != state.baudrate) {
 474
 475                     // Try switching baudrates
 476                     uint32_t portBaudrate = baudRates[portConfig->peripheral_baudrateIndex];
 477                     FRSKY_OSD_TRACE("requesting baudrate switch from %u to %u",
 478                         (unsigned)baudRates[state.baudrate], (unsigned)portBaudrate);
 479                     frskyOSDSendAsyncCommand(OSD_CMD_SET_DATA_RATE, &portBaudrate, sizeof(portBaudrate));
 480                     frskyOSDFlushSendBuffer();
 481                     return true;
 482                 }
 483             }
 484             state.info.major = resp->versionMajor;
 485             state.info.minor = resp->versionMinor;
 486             state.info.grid.rows = resp->gridRows;
 487             state.info.grid.columns = resp->gridColumns;
 488             state.info.viewport.width = resp->pixelWidth;
 489             state.info.viewport.height = resp->pixelHeight;
 490             if (!state.initialized) {
 491                 FRSKY_OSD_DEBUG("initialized. Version %u.%u.%u, pixels=%ux%u, grid=%ux%u",
 492                     resp->versionMajor, resp->versionMinor, resp->versionPatch,
 493                     resp->pixelWidth, resp->pixelHeight, resp->gridColumns, resp->gridRows);
 494                 state.initialized = true;
 495                 frskyOSDClearScreen();
 496                 frskyOSDResetDrawingState();
 497             }
 498             return true;
 499         }
 500         case OSD_CMD_READ_FONT:
 501         {
 502             if (!state.recvOsdCharacter.chr) {
 503                 FRSKY_OSD_DEBUG("Got unexpected font character");
 504                 break;
 505             }
 506             if (size < sizeof(uint16_t) + FRSKY_OSD_CHAR_TOTAL_BYTES) {
 507                 FRSKY_OSD_TRACE("Received buffer too small for a character: %u bytes", size);
 508                 break;
 509             }
 510             const frskyOSDCharacter_t *chr = payload;
 511             state.recvOsdCharacter.addr = chr->addr;
 512             FRSKY_OSD_TRACE("Received character %u", chr->addr);
 513             // Skip character address
 514             memcpy(state.recvOsdCharacter.chr->data, &chr->data, MIN(sizeof(state.recvOsdCharacter.chr->data), (size_t)FRSKY_OSD_CHAR_TOTAL_BYTES));
 515             return true;
 516         }
 517         case OSD_CMD_WRITE_FONT:
 518         {
 519             // We only wait for the confirmation, we're not interested in the data
 520             return true;
 521         }
 522         case OSD_CMD_WIDGET_SET_CONFIG:
 523         {
 524             return true;
 525         }
 526         case OSD_CMD_SET_DATA_RATE:
 527         {
 528             if (size < sizeof(uint32_t)) {
 529                 break;
 530             }
 531             const uint32_t *newBaudrate = payload;
 532             if (*newBaudrate && *newBaudrate != baudRates[state.baudrate]) {
 533                 FRSKY_OSD_TRACE("changed baudrate from %u to %u", (unsigned)baudRates[state.baudrate],
 534                                 (unsigned)*newBaudrate);
 535                 serialSetBaudRate(state.port, *newBaudrate);
 536                 // OSD might have returned a different baudrate from our
 537                 // predefined ones. Be ready to handle that
 538                 state.baudrate = 0;
 539                 for (baudRate_e ii = 0; ii <= BAUD_MAX; ii++) {
 540                     if (baudRates[ii] == *newBaudrate) {
 541                         state.baudrate = ii;
 542                         break;
 543                     }
 544                 }
 545             } else {
 546                 FRSKY_OSD_TRACE("baudrate refused, returned %u", (unsigned)*newBaudrate);
 547             }
 548             // Make sure we request OSD_CMD_INFO again as soon
 549             // as possible and don't try to change the baudrate
 550             // anymore.
 551             state.nextInfoRequest = 0;
 552             state.keepBaudrate = true;
 553             return true;
 554         }
 555         default:
 556             break;
 557     }
 558     return false;
 559 }
 560
 561 static bool frskyOSDDispatchResponse(void)
 562 {
 563     const uint8_t *payload = state.recvBuffer.data;
 564     int remaining = (int)state.recvBuffer.pos;
 565     bool ok = false;
 566     if (remaining > 0) {
 567         // OSD sends commands one by one, so we don't need to handle
 568         // a frame with multiple ones.
 569         uint8_t cmd = *payload;
 570         payload++;
 571         remaining--;
 572         if (frskyOSDHandleCommand(cmd, payload, remaining)) {
 573             ok = true;
 574         } else {
 575             FRSKY_OSD_DEBUG("Discarding buffer due to unhandled command %u (%d bytes remaining)", cmd, remaining);
 576         }
 577     }
 578     frskyOSDResetReceiveBuffer();
 579     return ok;
 580 }
 581
 582 static bool frskyOSDShouldRequestInfo(void)
 583 {
 584     return !frskyOSDIsReady() || millis() > state.nextInfoRequest;
 585 }
 586
 587 static void frskyOSDRequestInfo(void)
 588 {
 589     timeMs_t now = millis();
 590     if (state.info.nextRequest < now) {
 591         timeMs_t interval;
 592         if (frskyOSDIsReady()) {
 593             // We already contacted the OSD, so we're just
 594             // polling it to see if the video changed.
 595             interval = FRSKY_OSD_INFO_READY_INTERVAL_MS;
 596         } else {
 597             // We haven't yet heard from the OSD. If this is not
 598             // the first request, switch to the next baudrate.
 599             if (state.info.nextRequest > 0 && !state.keepBaudrate) {
 600                 if (state.baudrate == BAUD_MAX) {
 601                     state.baudrate = FRSKY_OSD_DEFAULT_BAUDRATE_INDEX;
 602                 } else {
 603                     state.baudrate++;
 604                 }
 605                 serialSetBaudRate(state.port, baudRates[state.baudrate]);
 606             }
 607             interval = FRSKY_OSD_INFO_INTERVAL_MS;
 608         }
 609         state.info.nextRequest = now + interval;
 610
 611         uint8_t version = FRSKY_OSD_SUPPORTED_API_VERSION;
 612         FRSKY_OSD_TRACE("request OSD_CMD_INFO at %u", (unsigned)baudRates[state.baudrate]);
 613         frskyOSDSendAsyncCommand(OSD_CMD_INFO, &version, sizeof(version));
 614         frskyOSDFlushSendBuffer();
 615     }
 616 }
 617
 618 static bool frskyOSDOpenPort(baudRate_e baudrate)
 619 {
 620     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_FRSKY_OSD);
 621     if (portConfig) {
 622         FRSKY_OSD_TRACE("configured, trying to connect...");
 623         portOptions_t portOptions = SERIAL_STOPBITS_1 | SERIAL_PARITY_NO;
 624         serialPort_t *port = openSerialPort(portConfig->identifier,
 625             FUNCTION_FRSKY_OSD, NULL, NULL, baudRates[baudrate],
 626             MODE_RXTX, portOptions);
 627
 628         if (port) {
 629             frskyOSDStateReset(port, baudrate);
 630             frskyOSDRequestInfo();
 631             return true;
 632         }
 633     }
 634     return false;
 635 }
 636
 637 static uint8_t frskyOSDEncodeAttr(textAttributes_t attr)
 638 {
 639     uint8_t frskyOSDAttr = 0;
 640     if (TEXT_ATTRIBUTES_HAVE_INVERTED(attr)) {
 641         frskyOSDAttr |= FRSKY_OSD_CHAR_ATTRIBUTE_COLOR_INVERSE;
 642     }
 643     if (TEXT_ATTRIBUTES_HAVE_SOLID_BG(attr)) {
 644         frskyOSDAttr |= FRSKY_OSD_CHAR_ATTRIBUTE_SOLID_BACKGROUND;
 645     }
 646     return frskyOSDAttr;
 647 }
 648
 649 bool frskyOSDInit(videoSystem_e videoSystem)
 650 {
 651     UNUSED(videoSystem);
 652
 653     return frskyOSDOpenPort(FRSKY_OSD_DEFAULT_BAUDRATE_INDEX);
 654 }
 655
 656 bool frskyOSDIsReady(void)
 657 {
 658     return state.info.minor > 0 || state.info.major > 0;
 659 }
 660
 661 void frskyOSDUpdate(void)
 662 {
 663     if (!state.port) {
 664         return;
 665     }
 666     frskyOSDUpdateReceiveBuffer();
 667
 668     if (frskyOSDIsResponseAvailable()) {
 669         frskyOSDDispatchResponse();
 670     }
 671
 672     if (frskyOSDShouldRequestInfo()) {
 673         frskyOSDRequestInfo();
 674     }
 675 }
 676
 677 void frskyOSDBeginTransaction(frskyOSDTransactionOptions_e opts)
 678 {
 679     if (opts & FRSKY_OSD_TRANSACTION_OPT_PROFILED) {
 680         frskyOSDPoint_t p = { .x = 0, .y = 10};
 681         frskyOSDSendAsyncCommand(OSD_CMD_TRANSACTION_BEGIN_PROFILED, &p, sizeof(p));
 682         if (opts & FRSKY_OSD_TRANSACTION_OPT_RESET_DRAWING) {
 683             frskyOSDResetDrawingState();
 684         }
 685     } else if (opts & FRSKY_OSD_TRANSACTION_OPT_RESET_DRAWING) {
 686         frskyOSDSendAsyncCommand(OSD_CMD_TRANSACTION_BEGIN_RESET_DRAWING, NULL, 0);
 687     } else {
 688         frskyOSDSendAsyncCommand(OSD_CMD_TRANSACTION_BEGIN, NULL, 0);
 689     }
 690 }
 691
 692 void frskyOSDCommitTransaction(void)
 693 {
 694     // Check wether the only command in the queue is a transaction begin.
 695     // In that, case, discard the send buffer since it will make generate
 696     // an empty transaction.
 697     if (state.sendBuffer.pos == 1) {
 698         if (state.sendBuffer.data[0] == OSD_CMD_TRANSACTION_BEGIN ||
 699             state.sendBuffer.data[0] == OSD_CMD_TRANSACTION_BEGIN_RESET_DRAWING) {
 700
 701             state.sendBuffer.pos = 0;
 702             return;
 703         }
 704     }
 705     frskyOSDSendAsyncCommand(OSD_CMD_TRANSACTION_COMMIT, NULL, 0);
 706     frskyOSDFlushSendBuffer();
 707 }
 708
 709 void frskyOSDFlushSendBuffer(void)
 710 {
 711     if (state.sendBuffer.pos > 0) {
 712         frskyOSDProcessCommandU8(NULL, FRSKY_OSD_PREAMBLE_BYTE_0);
 713         frskyOSDProcessCommandU8(NULL, FRSKY_OSD_PREAMBLE_BYTE_1);
 714
 715         uint8_t crc = 0;
 716         uint8_t buffer[4];
 717         int lengthSize = uvarintEncode(state.sendBuffer.pos, buffer, sizeof(buffer));
 718         for (int ii = 0; ii < lengthSize; ii++) {
 719             frskyOSDProcessCommandU8(&crc, buffer[ii]);
 720         }
 721         for (unsigned ii = 0; ii < state.sendBuffer.pos; ii++) {
 722             frskyOSDProcessCommandU8(&crc, state.sendBuffer.data[ii]);
 723         }
 724         frskyOSDProcessCommandU8(NULL, crc);
 725         state.sendBuffer.pos = 0;
 726     }
 727 }
 728
 729 bool frskyOSDReadFontCharacter(unsigned char_address, osdCharacter_t *chr)
 730 {
 731     if (!frskyOSDIsReady()) {
 732         return false;
 733     }
 734
 735     uint16_t addr = char_address;
 736
 737     state.recvOsdCharacter.addr = UINT16_MAX;
 738     state.recvOsdCharacter.chr = chr;
 739
 740     // 500ms should be more than enough to receive ~70 bytes @ 115200 bps
 741     bool ok = frskyOSDSendSyncCommand(OSD_CMD_READ_FONT, &addr, sizeof(addr), 500);
 742
 743     state.recvOsdCharacter.chr = NULL;
 744
 745     if (ok && state.recvOsdCharacter.addr == addr) {
 746         return true;
 747     }
 748     return false;
 749 }
 750
 751 bool frskyOSDWriteFontCharacter(unsigned char_address, const osdCharacter_t *chr)
 752 {
 753     if (!frskyOSDIsReady()) {
 754         return false;
 755     }
 756
 757     frskyOSDCharacter_t c;
 758     STATIC_ASSERT(sizeof(*chr) == sizeof(c.data), invalid_character_size);
 759
 760     memcpy(&c.data, chr, sizeof(c.data));
 761     c.addr = char_address;
 762     FRSKY_OSD_TRACE("Writing font character %u", char_address);
 763     frskyOSDSendSyncCommand(OSD_CMD_WRITE_FONT, &c, sizeof(c), 1000);
 764     return true;
 765 }
 766
 767 unsigned frskyOSDGetGridRows(void)
 768 {
 769     return state.info.grid.rows;
 770 }
 771
 772 unsigned frskyOSDGetGridCols(void)
 773 {
 774     return state.info.grid.columns;
 775 }
 776
 777 unsigned frskyOSDGetPixelWidth(void)
 778 {
 779     return state.info.viewport.width;
 780 }
 781
 782 unsigned frskyOSDGetPixelHeight(void)
 783 {
 784     return state.info.viewport.height;
 785 }
 786
 787 static void frskyOSDSendAsyncBlobCommand(uint8_t cmd, const void *header, size_t headerSize, const void *blob, size_t blobSize, bool explicitBlobSize)
 788 {
 789     uint8_t payload[128];
 790
 791     memcpy(payload, header, headerSize);
 792
 793     int uvarintSize;
 794     if (explicitBlobSize) {
 795         uvarintSize = uvarintEncode(blobSize, &payload[headerSize], sizeof(payload) - headerSize);
 796     } else {
 797         uvarintSize = 0;
 798     }
 799     memcpy(&payload[headerSize + uvarintSize], blob, blobSize);
 800     frskyOSDSendAsyncCommand(cmd, payload,  headerSize + uvarintSize + blobSize);
 801 }
 802
 803 static void frskyOSDSendAsyncBlobWithExplicitSizeCommand(uint8_t cmd, const void *header, size_t headerSize, const void *blob, size_t blobSize)
 804 {
 805     frskyOSDSendAsyncBlobCommand(cmd, header, headerSize, blob, blobSize, true);
 806 }
 807
 808 static void frskyOSDSendAsyncBlobWithoutExplicitSizeCommand(uint8_t cmd, const void *header, size_t headerSize, const void *blob, size_t blobSize)
 809 {
 810     frskyOSDSendAsyncBlobCommand(cmd, header, headerSize, blob, blobSize, false);
 811 }
 812
 813 static void frskyOSDSendCharInGrid_V1(unsigned x, unsigned y, uint16_t chr, textAttributes_t attr)
 814 {
 815     uint8_t payload[] = {
 816         x,
 817         y,
 818         chr & 0xFF,
 819         chr >> 8,
 820         frskyOSDEncodeAttr(attr),
 821     };
 822     frskyOSDSendAsyncCommand(OSD_CMD_DRAW_GRID_CHR, payload, sizeof(payload));
 823 }
 824
 825 static void frskyOSDSendCharInGrid_V2(unsigned x, unsigned y, uint16_t chr, textAttributes_t attr)
 826 {
 827     frskyOSDDrawGridChrV2Cmd_t payload = {
 828         .gx = x,
 829         .gy = y,
 830         .chr = chr,
 831         .opts = frskyOSDEncodeAttr(attr),
 832     };
 833     frskyOSDSendAsyncCommand(OSD_CMD_DRAW_GRID_CHR_2, &payload, sizeof(payload));
 834 }
 835
 836 static void frskyOSDSendCharInGrid(unsigned x, unsigned y, uint16_t chr, textAttributes_t attr)
 837 {
 838     if (frskyOSDSpeaksV2()) {
 839         frskyOSDSendCharInGrid_V2(x, y, chr, attr);
 840     } else {
 841         frskyOSDSendCharInGrid_V1(x, y, chr, attr);
 842     }
 843 }
 844
 845 static void frskyOSDSendCharSendStringInGrid_V1(unsigned x, unsigned y, const char *buff, textAttributes_t attr)
 846 {
 847     frskyOSDDrawGridStrHeaderCmd_t cmd = {
 848         .gx = x,
 849         .gy = y,
 850         .opts = frskyOSDEncodeAttr(attr),
 851     };
 852     frskyOSDSendAsyncBlobWithExplicitSizeCommand(OSD_CMD_DRAW_GRID_STR, &cmd, sizeof(cmd), buff, strlen(buff) + 1);
 853 }
 854
 855 static void frskyOSDSendCharSendStringInGrid_V2(unsigned x, unsigned y, const char *buff, textAttributes_t attr)
 856 {
 857     frskyOSDDrawGridStrV2HeaderCmd_t cmd = {
 858         .gx = x,
 859         .gy = y,
 860         .opts = frskyOSDEncodeAttr(attr),
 861     };
 862     size_t len = strlen(buff);
 863     if (len <= 15) {
 864         cmd.size = len;
 865         frskyOSDSendAsyncBlobWithoutExplicitSizeCommand(OSD_CMD_DRAW_GRID_STR_2, &cmd, sizeof(cmd), buff, len);
 866     } else {
 867         frskyOSDSendAsyncBlobWithExplicitSizeCommand(OSD_CMD_DRAW_GRID_STR_2, &cmd, sizeof(cmd), buff, len);
 868     }
 869 }
 870
 871 static void frskyOSDSendCharSendStringInGrid(unsigned x, unsigned y, const char *buff, textAttributes_t attr)
 872 {
 873     if (frskyOSDSpeaksV2()) {
 874         frskyOSDSendCharSendStringInGrid_V2(x, y, buff, attr);
 875     } else {
 876         frskyOSDSendCharSendStringInGrid_V1(x, y, buff, attr);
 877     }
 878 }
 879
 880 void frskyOSDDrawStringInGrid(unsigned x, unsigned y, const char *buff, textAttributes_t attr)
 881 {
 882     unsigned charsUpdated = 0;
 883     const char *updatedCharAt = NULL;
 884     uint16_t *entry = osdCharacterGridBufferGetEntryPtr(x, y);
 885     const char *p;
 886     unsigned xx;
 887     for (p = buff, xx = x; *p && xx < state.info.grid.columns; p++, entry++, xx++) {
 888         unsigned val = FRSKY_OSD_GRID_BUFFER_ENCODE(*p, attr);
 889         if (*entry != val) {
 890             if (++charsUpdated == 1) {
 891                 // First character that needs to be updated, save it
 892                 // in case we can issue a single update.
 893                 updatedCharAt = p;
 894             }
 895             *entry = val;
 896         }
 897     }
 898
 899     if (charsUpdated == 0) {
 900         return;
 901     }
 902
 903     if (charsUpdated == 1) {
 904         frskyOSDSendCharInGrid(x + (updatedCharAt - buff), y, *updatedCharAt, attr);
 905         return;
 906     }
 907
 908     frskyOSDSendCharSendStringInGrid(x, y, buff, attr);
 909 }
 910
 911 void frskyOSDDrawCharInGrid(unsigned x, unsigned y, uint16_t chr, textAttributes_t attr)
 912 {
 913     uint16_t *entry = osdCharacterGridBufferGetEntryPtr(x, y);
 914     unsigned val = FRSKY_OSD_GRID_BUFFER_ENCODE(chr, attr);
 915
 916     if (*entry == val) {
 917         return;
 918     }
 919
 920     frskyOSDSendCharInGrid(x, y, chr, attr);
 921
 922     *entry = val;
 923 }
 924
 925 bool frskyOSDReadCharInGrid(unsigned x, unsigned y, uint16_t *c, textAttributes_t *attr)
 926 {
 927     uint16_t val = *osdCharacterGridBufferGetEntryPtr(x, y);
 928     // We use the lower 10 bits for characters
 929     *c = val & FRSKY_OSD_GRID_BUFFER_CHAR_MASK;
 930     *attr = val >> FRSKY_OSD_GRID_BUFFER_CHAR_BITS;
 931     return true;
 932 }
 933
 934 void frskyOSDClearScreen(void)
 935 {
 936     osdCharacterGridBufferClear();
 937     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_CLEAR_SCREEN, NULL, 0);
 938 }
 939
 940 void frskyOSDSetStrokeColor(frskyOSDColor_e color)
 941 {
 942     uint8_t c = color;
 943     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_SET_STROKE_COLOR, &c, sizeof(c));
 944 }
 945
 946 void frskyOSDSetFillColor(frskyOSDColor_e color)
 947 {
 948     uint8_t c = color;
 949     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_SET_FILL_COLOR, &c, sizeof(c));
 950 }
 951
 952 void frskyOSDSetStrokeAndFillColor(frskyOSDColor_e color)
 953 {
 954     uint8_t c = color;
 955     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_SET_STROKE_AND_FILL_COLOR, &c, sizeof(c));
 956 }
 957
 958 void frskyOSDSetColorInversion(bool inverted)
 959 {
 960     uint8_t c = inverted ? 1 : 0;
 961     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_SET_COLOR_INVERSION, &c, sizeof(c));
 962 }
 963
 964 void frskyOSDSetPixel(int x, int y, frskyOSDColor_e color)
 965 {
 966     frskyOSDSetPixel_t sp = {.p = {.x = x, .y = y}, .color = color};
 967     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_SET_PIXEL, &sp, sizeof(sp));
 968 }
 969
 970 void frskyOSDSetPixelToStrokeColor(int x, int y)
 971 {
 972     frskyOSDPoint_t p = { .x = x, .y = y};
 973     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_SET_PIXEL_TO_STROKE_COLOR, &p, sizeof(p));
 974 }
 975
 976 void frskyOSDSetPixelToFillColor(int x, int y)
 977 {
 978     frskyOSDPoint_t p = { .x = x, .y = y};
 979     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_SET_PIXEL_TO_FILL_COLOR, &p, sizeof(p));
 980 }
 981
 982 void frskyOSDSetStrokeWidth(unsigned width)
 983 {
 984     uint8_t w = width;
 985     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_SET_STROKE_WIDTH, &w, sizeof(w));
 986 }
 987
 988 void frskyOSDSetLineOutlineType(frskyOSDLineOutlineType_e outlineType)
 989 {
 990     uint8_t type = outlineType;
 991     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_SET_LINE_OUTLINE_TYPE, &type, sizeof(type));
 992 }
 993
 994 void frskyOSDSetLineOutlineColor(frskyOSDColor_e outlineColor)
 995 {
 996     uint8_t color = outlineColor;
 997     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_SET_LINE_OUTLINE_COLOR, &color, sizeof(color));
 998 }
 999
1000 void frskyOSDClipToRect(int x, int y, int w, int h)
1001 {
1002     frskyOSDRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
1003     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_CLIP_TO_RECT, &r, sizeof(r));
1004 }
1005
1006 void frskyOSDClearRect(int x, int y, int w, int h)
1007 {
1008     frskyOSDRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
1009     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_CLEAR_RECT, &r, sizeof(r));
1010 }
1011
1012 void frskyOSDResetDrawingState(void)
1013 {
1014     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_RESET, NULL, 0);
1015 }
1016
1017 void frskyOSDDrawCharacter(int x, int y, uint16_t chr, uint8_t opts)
1018 {
1019     frskyOSDDrawCharacterCmd_t dc = { .p = {.x = x, .y = y}, .chr = chr, .opts = opts};
1020     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_DRAW_CHAR, &dc, sizeof(dc));
1021 }
1022
1023 void frskyOSDDrawCharacterMask(int x, int y, uint16_t chr, frskyOSDColor_e color, uint8_t opts)
1024 {
1025     frskyOSDDrawCharacterMaskCmd_t dc = { .dc = { .p = {.x = x, .y = y}, .chr = chr, .opts = opts}, .maskColor = color};
1026     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_DRAW_CHAR_MASK, &dc, sizeof(dc));
1027 }
1028
1029 void frskyOSDDrawString(int x, int y, const char *s, uint8_t opts)
1030 {
1031     frskyOSDDrawStrCommandHeaderCmd_t cmd;
1032     cmd.p.x = x;
1033     cmd.p.y = y;
1034     cmd.opts = opts;
1035
1036     frskyOSDSendAsyncBlobWithExplicitSizeCommand(OSD_CMD_DRAWING_DRAW_STRING, &cmd, sizeof(cmd), s, strlen(s) + 1);
1037 }
1038
1039 void frskyOSDDrawStringMask(int x, int y, const char *s, frskyOSDColor_e color, uint8_t opts)
1040 {
1041     frskyOSDDrawStrMaskCommandHeaderCmd_t cmd;
1042     cmd.p.x = x;
1043     cmd.p.y = y;
1044     cmd.opts = opts;
1045     cmd.maskColor = color;
1046
1047     frskyOSDSendAsyncBlobWithExplicitSizeCommand(OSD_CMD_DRAWING_DRAW_STRING_MASK, &cmd, sizeof(cmd), s, strlen(s) + 1);
1048 }
1049
1050 void frskyOSDMoveToPoint(int x, int y)
1051 {
1052     frskyOSDPoint_t p = { .x = x, .y = y};
1053     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_MOVE_TO_POINT, &p, sizeof(p));
1054 }
1055
1056 void frskyOSDStrokeLineToPoint(int x, int y)
1057 {
1058     frskyOSDPoint_t p = { .x = x, .y = y};
1059     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_STROKE_LINE_TO_POINT, &p, sizeof(p));
1060 }
1061
1062 void frskyOSDStrokeTriangle(int x1, int y1, int x2, int y2, int x3, int y3)
1063 {
1064     frskyOSDTriangle_t t = {.p1 = {.x = x1, .y = y1}, .p2 = {.x = x2, .y = y2}, .p3 = { .x = x3, .y = y3}};
1065     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_STROKE_TRIANGLE, &t, sizeof(t));
1066 }
1067
1068 void frskyOSDFillTriangle(int x1, int y1, int x2, int y2, int x3, int y3)
1069 {
1070     frskyOSDTriangle_t t = {.p1 = {.x = x1, .y = y1}, .p2 = {.x = x2, .y = y2}, .p3 = { .x = x3, .y = y3}};
1071     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_FILL_TRIANGLE, &t, sizeof(t));
1072 }
1073
1074 void frskyOSDFillStrokeTriangle(int x1, int y1, int x2, int y2, int x3, int y3)
1075 {
1076     frskyOSDTriangle_t t = {.p1 = {.x = x1, .y = y1}, .p2 = {.x = x2, .y = y2}, .p3 = { .x = x3, .y = y3}};
1077     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_FILL_STROKE_TRIANGLE, &t, sizeof(t));
1078 }
1079
1080 void frskyOSDStrokeRect(int x, int y, int w, int h)
1081 {
1082     frskyOSDRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
1083     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_STROKE_RECT, &r, sizeof(r));
1084 }
1085
1086 void frskyOSDFillRect(int x, int y, int w, int h)
1087 {
1088     frskyOSDRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
1089     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_FILL_RECT, &r, sizeof(r));
1090 }
1091
1092 void frskyOSDFillStrokeRect(int x, int y, int w, int h)
1093 {
1094     frskyOSDRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
1095     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_FILL_STROKE_RECT, &r, sizeof(r));
1096 }
1097
1098 void frskyOSDStrokeEllipseInRect(int x, int y, int w, int h)
1099 {
1100     frskyOSDRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
1101     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_STROKE_ELLIPSE_IN_RECT, &r, sizeof(r));
1102 }
1103
1104 void frskyOSDFillEllipseInRect(int x, int y, int w, int h)
1105 {
1106     frskyOSDRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
1107     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_FILL_ELLIPSE_IN_RECT, &r, sizeof(r));
1108 }
1109
1110 void frskyOSDFillStrokeEllipseInRect(int x, int y, int w, int h)
1111 {
1112     frskyOSDRect_t r = { .origin = { .x = x, .y = y}, .size = {.w = w, .h = h}};
1113     frskyOSDSendAsyncCommand(OSD_CMD_DRAWING_FILL_STROKE_ELLIPSE_IN_RECT, &r, sizeof(r));
1114 }
1115
1116 void frskyOSDCtmReset(void)
1117 {
1118     frskyOSDSendAsyncCommand(OSD_CMD_CTM_RESET, NULL, 0);
1119 }
1120
1121 void frskyOSDCtmSet(float m11, float m12, float m21, float m22, float m31, float m32)
1122 {
1123     float values[] = {
1124         m11, m12,
1125         m21, m22,
1126         m31, m32,
1127     };
1128     frskyOSDSendAsyncCommand(OSD_CMD_CTM_SET, values, sizeof(values));
1129 }
1130
1131 void frskyOSDCtmTranslate(float tx, float ty)
1132 {
1133     float values[] = {
1134         tx,
1135         ty,
1136     };
1137     frskyOSDSendAsyncCommand(OSD_CMD_CTM_TRANSLATE, values, sizeof(values));
1138 }
1139
1140 void frskyOSDCtmScale(float sx, float sy)
1141 {
1142     float values[] = {
1143         sx,
1144         sy,
1145     };
1146     frskyOSDSendAsyncCommand(OSD_CMD_CTM_SCALE, values, sizeof(values));
1147 }
1148
1149 void frskyOSDCtmRotate(float r)
1150 {
1151     frskyOSDSendAsyncCommand(OSD_CMD_CTM_ROTATE, &r, sizeof(r));
1152 }
1153
1154 void frskyOSDContextPush(void)
1155 {
1156     frskyOSDSendAsyncCommand(OSD_CMD_CONTEXT_PUSH, NULL, 0);
1157 }
1158
1159 void frskyOSDContextPop(void)
1160 {
1161     frskyOSDSendAsyncCommand(OSD_CMD_CONTEXT_POP, NULL, 0);
1162 }
1163
1164 bool frskyOSDSupportsWidgets(void)
1165 {
1166     return frskyOSDSpeaksV2();
1167 }
1168
1169 bool frskyOSDSetWidgetConfig(frskyOSDWidgetID_e widget, const void *config, size_t configSize)
1170 {
1171     if (!frskyOSDSupportsWidgets()) {
1172         return false;
1173     }
1174
1175     uint8_t buffer[configSize + 1];
1176     buffer[0] = widget;
1177     memcpy(buffer + 1, config, configSize);
1178     bool ok = frskyOSDSendSyncCommand(OSD_CMD_WIDGET_SET_CONFIG, buffer, sizeof(buffer), 500);
1179     return ok && state.error.code == 0;
1180 }
1181
1182 bool frskyOSDDrawWidget(frskyOSDWidgetID_e widget, const void *data, size_t dataSize)
1183 {
1184     if (!frskyOSDSupportsWidgets()) {
1185         return false;
1186     }
1187
1188     uint8_t buffer[dataSize + 1];
1189     buffer[0] = widget;
1190     memcpy(buffer + 1, data, dataSize);
1191     frskyOSDSendAsyncCommand(OSD_CMD_WIDGET_DRAW, buffer, sizeof(buffer));
1192     return true;
1193 }
1194
1195 uint32_t frskyOSDQuantize(float val, float min, float max, int bits)
1196 {
1197     if (val < min) {
1198         val = max - (min - val);
1199     } else if (val > max) {
1200         val = min + (val - max);
1201     }
1202     return (val * (1 << bits)) / max;
1203 }
1204
1205 #endif