?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
updates
[inav.git] / src / main / io / frsky_osd.c
bloba69df4e104c4cba6ecf885c85f6b70c17f0c26d9
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
INAV: Navigation-enabled flight control software