Line data Source code
1 : /*
2 : * This file is free software: you can redistribute it and/or modify it
3 : * under the terms of the GNU General Public License as published by the
4 : * Free Software Foundation, either version 3 of the License, or
5 : * (at your option) any later version.
6 : *
7 : * This file is distributed in the hope that it will be useful, but
8 : * WITHOUT ANY WARRANTY; without even the implied warranty of
9 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
10 : * See the GNU General Public License for more details.
11 : *
12 : * You should have received a copy of the GNU General Public License along
13 : * with this program. If not, see <http://www.gnu.org/licenses/>.
14 : *
15 : */
16 : /*
17 : OSD backend for MSP
18 : */
19 : #include <AP_MSP/AP_MSP.h>
20 : #include <AP_MSP/msp.h>
21 : #include "AP_OSD_MSP_DisplayPort.h"
22 :
23 : #if HAL_WITH_MSP_DISPLAYPORT
24 :
25 : #include <GCS_MAVLink/GCS.h>
26 :
27 : static const struct AP_Param::defaults_table_struct defaults_table[] = {
28 : /*
29 : { "PARAM_NAME", value_float }
30 : */
31 : };
32 :
33 : extern const AP_HAL::HAL &hal;
34 : constexpr uint8_t AP_OSD_MSP_DisplayPort::symbols[AP_OSD_NUM_SYMBOLS];
35 :
36 : // initialise backend
37 2 : bool AP_OSD_MSP_DisplayPort::init(void)
38 : {
39 : // check if we have a DisplayPort backend to use
40 2 : const AP_MSP *msp = AP::msp();
41 2 : if (msp == nullptr) {
42 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"MSP backend not available");
43 0 : return false;
44 : }
45 2 : _displayport = msp->find_protocol(AP_SerialManager::SerialProtocol_MSP_DisplayPort);
46 2 : if (_displayport == nullptr) {
47 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"MSP DisplayPort uart not available");
48 0 : return false;
49 : }
50 : // re-init port here for use in this thread
51 2 : _displayport->init_uart();
52 2 : return true;
53 : }
54 :
55 : // called by the OSD thread once
56 2 : void AP_OSD_MSP_DisplayPort::osd_thread_run_once()
57 : {
58 2 : if (_displayport != nullptr) {
59 2 : _displayport->init_uart();
60 : }
61 2 : }
62 :
63 548 : void AP_OSD_MSP_DisplayPort::clear(void)
64 : {
65 : // check if we need to enable some options
66 : // but only for actual OSD screens
67 548 : if (_osd.get_current_screen() < AP_OSD_NUM_DISPLAY_SCREENS) {
68 548 : const uint8_t txt_resolution = _osd.screen[_osd.get_current_screen()].get_txt_resolution();
69 548 : const uint8_t font_index = _osd.screen[_osd.get_current_screen()].get_font_index();
70 548 : _displayport->msp_displayport_set_options(font_index, txt_resolution);
71 : }
72 :
73 : // clear remote MSP screen
74 548 : _displayport->msp_displayport_clear_screen();
75 :
76 : // toggle flashing @1Hz
77 548 : const uint32_t now = AP_HAL::millis();
78 548 : if ((uint32_t(now * 0.004) & 0x01) != _blink_on) {
79 223 : _blink_on = !_blink_on;
80 223 : blink_phase = (blink_phase+1)%4;
81 : }
82 548 : }
83 :
84 18843 : void AP_OSD_MSP_DisplayPort::write(uint8_t x, uint8_t y, const char* text)
85 : {
86 : #if AP_MSP_INAV_FONTS_ENABLED
87 18843 : const AP_MSP *msp = AP::msp();
88 18843 : if (msp && msp->is_option_enabled(AP_MSP::Option::DISPLAYPORT_INAV_SYMBOLS)) {
89 0 : write_INAV(x, y, text);
90 0 : return;
91 : }
92 : #endif
93 18843 : _displayport->msp_displayport_write_string(x, y, false, text, 0);
94 : }
95 :
96 : #if AP_MSP_INAV_FONTS_ENABLED
97 0 : void AP_OSD_MSP_DisplayPort::write_INAV(uint8_t x, uint8_t y, const char* text)
98 : {
99 0 : uint8_t idx = 0;
100 0 : uint8_t buf_idx = 0;
101 0 : uint8_t last_inav_table = 0xFF; // 0xFF == unknown
102 0 : uint8_t x_offset = 0;
103 0 : const uint8_t max_idx = MIN((uint8_t)(strlen(text)-1), DISPLAYPORT_WRITE_BUFFER_MAX_LEN-2);
104 : uint8_t inav_symbol;
105 : uint8_t inav_table;
106 0 : while (text[idx] != 0 && idx <= max_idx)
107 : {
108 : // transcode
109 0 : inav_symbol = ap_to_inav_symbols_map[(uint8_t)text[idx]][0];
110 0 : inav_table = ap_to_inav_symbols_map[(uint8_t)text[idx]][1];
111 :
112 0 : if (last_inav_table == 0xFF) {
113 0 : last_inav_table = inav_table;
114 : }
115 :
116 0 : if (inav_table != last_inav_table) {
117 0 : displayport_write_buffer[buf_idx] = 0x00; // add a terminator
118 0 : _displayport->msp_displayport_write_string(x + x_offset, y, false, displayport_write_buffer, last_inav_table);
119 0 : x_offset += buf_idx;
120 0 : buf_idx = 0;
121 0 : last_inav_table = inav_table;
122 : }
123 :
124 0 : displayport_write_buffer[buf_idx++] = inav_symbol;
125 :
126 0 : if (idx == max_idx) {
127 : // flush when we detect end of string
128 0 : displayport_write_buffer[buf_idx] = 0x00; // add a terminator
129 0 : _displayport->msp_displayport_write_string(x+x_offset, y, false, displayport_write_buffer, inav_table);
130 : }
131 0 : idx++;
132 : }
133 0 : }
134 : #endif
135 :
136 18409 : uint8_t AP_OSD_MSP_DisplayPort::format_string_for_osd(char* buff, uint8_t size, bool decimal_packed, const char *fmt, va_list ap)
137 : {
138 18409 : const AP_MSP *msp = AP::msp();
139 18409 : const bool pack = decimal_packed && msp && !msp->is_option_enabled(AP_MSP::Option::DISPLAYPORT_BTFL_SYMBOLS);
140 18409 : return AP_OSD_Backend::format_string_for_osd(buff, size, pack, fmt, ap);
141 : }
142 :
143 548 : void AP_OSD_MSP_DisplayPort::flush(void)
144 : {
145 : // grab the screen and force a redraw
146 548 : _displayport->msp_displayport_grab();
147 548 : _displayport->msp_displayport_draw_screen();
148 :
149 : // ok done processing displayport data
150 : // let's process incoming MSP frames (and reply if needed)
151 548 : _displayport->process_incoming_data();
152 : // push any outgoing telemetry frames
153 548 : _displayport->process_outgoing_data();
154 548 : }
155 :
156 2 : void AP_OSD_MSP_DisplayPort::init_symbol_set(uint8_t *lookup_table, const uint8_t size)
157 : {
158 2 : const AP_MSP *msp = AP::msp();
159 : // do we use backend specific symbols table?
160 2 : if (msp && msp->is_option_enabled(AP_MSP::Option::DISPLAYPORT_BTFL_SYMBOLS)&& !msp->is_option_enabled(AP_MSP::Option::DISPLAYPORT_INAV_SYMBOLS)) {
161 0 : memcpy(lookup_table, symbols, size);
162 : } else {
163 2 : memcpy(lookup_table, AP_OSD_Backend::symbols, size);
164 : }
165 2 : }
166 :
167 : // override built in positions with defaults for MSP OSD
168 0 : void AP_OSD_MSP_DisplayPort::setup_defaults(void)
169 : {
170 0 : AP_Param::set_defaults_from_table(defaults_table, ARRAY_SIZE(defaults_table));
171 0 : }
172 :
173 2 : AP_OSD_Backend *AP_OSD_MSP_DisplayPort::probe(AP_OSD &osd)
174 : {
175 2 : AP_OSD_MSP_DisplayPort *backend = NEW_NOTHROW AP_OSD_MSP_DisplayPort(osd);
176 2 : if (!backend) {
177 0 : return nullptr;
178 : }
179 2 : if (!backend->init()) {
180 0 : delete backend;
181 0 : return nullptr;
182 : }
183 2 : return backend;
184 : }
185 :
186 : // return a correction factor used to display angles correctly
187 548 : float AP_OSD_MSP_DisplayPort::get_aspect_ratio_correction() const
188 : {
189 548 : return 12.0/18.0;
190 : }
191 :
192 :
193 : #if AP_MSP_INAV_FONTS_ENABLED
194 : /*
195 : the first column in the code ID, 2nd column is the code page
196 : */
197 : const uint8_t AP_OSD_MSP_DisplayPort::ap_to_inav_symbols_map[256][2] {
198 : {0x00,0},
199 : {0x01,0}, //SYM_RSSI
200 : {0x02,0},
201 : {0x03,0},
202 : {0x04,0},
203 : {0x05,0},
204 : {0x1F,0}, //SYM_VOLT
205 : {0x99,0}, //SYM_MAH
206 : {0x08,0},
207 : {0x3E,0}, //SYM_SIDEBAR_R_ARROW
208 : {0x3C,0}, //SYM_SIDEBAR_L_ARROW
209 : {0x0B,0},
210 : {0x0C,0},
211 : {0x96,0}, //SYM_DEGREES_F
212 : {0x97,0}, //SYM_DEGREES_C
213 : {0x74,0}, //SYM_FT
214 : {0x10,0},
215 : {0x2E,1}, //SYM_SIDEBAR_I
216 : {0x31,1}, //SYM_SIDEBAR_J
217 : {0x2E,1}, //SYM_SIDEBAR_A
218 : {0x31,1}, //SYM_SIDEBAR_B
219 : {0x2E,1}, //SYM_SIDEBAR_C
220 : {0x16,0}, //SYM_WIND
221 : {0x17,0},
222 : {0x4E,0}, //SYM_HEADING_N
223 : {0x53,0}, //SYM_HEADING_S
224 : {0x45,0}, //SYM_HEADING_E
225 : {0x57,0}, //SYM_HEADING_W
226 : {0x7C,0}, //SYM_HEADING_DIVIDED_LINE
227 : {0x7C,0}, //SYM_HEADING_LINE
228 : {0x09,0}, //SYM_SAT_L
229 : {0xE0,0}, //SYM_SAT_R
230 : // 00-1F above
231 : {0x20,0},
232 : {0x21,0},
233 : {0x75,0}, //SYM_DIST
234 : {0xD0,0}, //SYM_FLAP
235 : {0x24,0},
236 : {0x25,0}, //SYM_PCNT
237 : {0x5F,1}, //SYM_AH_CENTER_LINE_LEFT
238 : {0x5A,1}, //SYM_AH_CENTER_LINE_RIGHT
239 : {0x28,0}, //SYM_ROLL0
240 : {0x29,0},
241 : {0x2A,0},
242 : {0x2B,0},
243 : {0x2C,0},
244 : {0xAE,0},
245 : {0x2E,0},
246 : {0x2F,0},
247 : {0x30,0},
248 : {0x31,0},
249 : {0x32,0},
250 : {0x33,0},
251 : {0x34,0},
252 : {0x35,0},
253 : {0x36,0},
254 : {0x37,0},
255 : {0x38,0},
256 : {0x39,0},
257 : {0x3A,0},
258 : {0x3B,0},
259 : {0x3C,0},
260 : {0x3D,0},
261 : {0x3E,0},
262 : {0x3F,0},
263 : //20-3F above
264 : {0x40,0},
265 : {0x41,0},
266 : {0x42,0},
267 : {0x43,0},
268 : {0x44,0},
269 : {0x45,0},
270 : {0x46,0},
271 : {0x47,0},
272 : {0x48,0},
273 : {0x49,0},
274 : {0x4A,0},
275 : {0x4B,0}, //SYM_KILO
276 : {0x4C,0},
277 : {0x4D,0},
278 : {0x4E,0},
279 : {0x4F,0},
280 : {0x50,0},
281 : {0x51,0},
282 : {0x52,0},
283 : {0x53,0},
284 : {0x54,0},
285 : {0x55,0},
286 : {0x56,0},
287 : {0x57,0},
288 : {0x58,0},
289 : {0x59,0},
290 : {0x5A,0},
291 : {0x5B,0},
292 : {0x5C,0},
293 : {0x5D,0},
294 : {0x5E,0},
295 : {0x5F,0},
296 : //40-5F above
297 : {0x3C,1}, //SYM_ARROW_START,next 15 symbols for arrows
298 : {0x3D,1},
299 : {0x3E,1},
300 : {0x3F,1},
301 : {0x40,1},
302 : {0x41,1},
303 : {0x42,1},
304 : {0x43,1},
305 : {0x44,1},
306 : {0x45,1},
307 : {0x46,1},
308 : {0x47,1},
309 : {0x48,1},
310 : {0x49,1},
311 : {0x4A,1},
312 : {0x4B,1},
313 : {0x70,0},
314 : {0x71,0},
315 : {0x72,0},
316 : {0x73,0},
317 : {0x74,0},
318 : {0x75,0},
319 : {0x76,0},
320 : {0x77,0},
321 : {0x78,0},
322 : {0x79,0},
323 : {0x20,0}, //SYM_RADIUS
324 : {0x7B,0},
325 : {0xAE,0}, //SYM_PTCH0
326 : {0x7D,0},
327 : {0x66,1}, //SYM_AH_CENTER
328 : {0x7F,0},
329 : // 60-7F above
330 : {0x4C,1}, //SYM_AH_H_START, and next 8 symbols
331 : {0x4D,1},
332 : {0x4E,1},
333 : {0x4F,1},
334 : {0x50,1},
335 : {0x51,1},
336 : {0x52,1},
337 : {0x53,1},
338 : {0x54,1},
339 : {0x0C,0}, //SYM_HEADING
340 : {0x8A,0},
341 : {0x8B,0},
342 : {0x8C,0},
343 : {0x8D,0},
344 : {0x8E,0},
345 : {0x8F,0},
346 : {0x63,0}, //SYM_BATT_FULL, and next 6 symbols
347 : {0x64,0},
348 : {0x65,0},
349 : {0x66,0},
350 : {0x67,0},
351 : {0x68,0},
352 : {0x69,0},
353 : {0xDD,0}, //SYM_BATT_FULL
354 : {0x98,0},
355 : {0x8D,0}, //SYM_FS,SYM_FS
356 : {0x6A,0}, //SYM_AMP
357 : {0x9B,0},
358 : {0x9F,0}, //SYM_FLY
359 : {0x9D,0},
360 : {0x9E,0},
361 : {0x8F,0}, //SYM_MS
362 : //80-9F above
363 : {0xA0,0},
364 : {0x90,0}, //SYM_KMH
365 : {0x55,1}, //SYM_UP_UP
366 : {0x56,1}, //SYM_UP
367 : {0x57,1}, //SYM_DOWN
368 : {0x58,1}, //SYM_DOWN_DOWN
369 : {0x03,0}, //SYM_GPS_LAT
370 : {0x04,0}, //SYM_GPS_LONG
371 : {0x0B,0}, //SYM_DEGR
372 : {0x52,0}, //SYM_ROLL
373 : {0x20,0}, //SYM_DPS
374 : {0x6D,0}, //SYM_WH
375 : {0xAC,0},
376 : {0xAD,0},
377 : {0x71,0}, //SYM_WATT
378 : {0x50,0}, //SYM_PITCH
379 : {0x91,0}, //SYM_MPH
380 : {0x76,0}, //SYM_ALT_M
381 : {0xB2,0},
382 : {0x78,0}, //SYM_ALT_FT
383 : {0xB4,0},
384 : {0xB5,0},
385 : {0xB6,0},
386 : {0xB7,0},
387 : {0xB8,0},
388 : {0x82,0}, //SYM_M
389 : {0x83,0}, //SYM_KM
390 : {0x84,0}, //SYM_MI
391 : {0xA0,0}, //SYM_CLK
392 : {0x0E,0}, //SYM_HDOP_L
393 : {0x0F,0}, //SYM_HDOP_R
394 : {0x10,0}, //SYM_HOME
395 : //A0-BF above
396 : {0xA1,0}, //start of decimal pack
397 : {0xA2,0},
398 : {0xA3,0},
399 : {0xA4,0},
400 : {0xA5,0},
401 : {0xA6,0},
402 : {0xA7,0},
403 : {0xA8,0},
404 : {0xA9,0},
405 : {0xAA,0},
406 : {0x5A,1}, //SYM_HOME, next 5 symbols
407 : {0x5A,1},
408 : {0x5A,1},
409 : {0x5A,1},
410 : {0x5A,1},
411 : {0x5A,1},
412 : {0xB1,0}, //continuation of decimal pack
413 : {0xB2,0},
414 : {0xB3,0},
415 : {0xB4,0},
416 : {0xB5,0},
417 : {0xB6,0},
418 : {0xB7,0},
419 : {0xB8,0},
420 : {0xB9,0},
421 : {0xBA,0},
422 : {0x2E,1}, //SYM_SIDEBAR_G
423 : {0x2E,1}, //SYM_SIDEBAR_E
424 : {0x31,1}, //SYM_SIDEBAR_F
425 : {0x31,1}, //SYM_SIDEBAR_D
426 : {0x31,1}, //SYM_SIDEBAR_H
427 : {0xBC,0}, //SYM_G
428 : //C0-DF above
429 : {0x8B,0}, //SYM_RPM
430 : {0x8C,0}, //SYM_ASPD
431 : {0x20,0}, //SYM_GSPD
432 : {0x86,0}, //SYM_WSPD
433 : {0x5E,0}, //SYM_VSPD
434 : {0x57,0}, //SYM_WPNO
435 : {0x20,0}, //SYM_WPDIR
436 : {0x20,0}, //SYM_WPDST
437 : {0x8E,0}, //SYM_FTMIN
438 : {0x65,1}, //SYM_DISARMED
439 : {0xAF,0}, //SYM_ROLLR
440 : {0xAD,0}, //SYM_ROLLL
441 : {0x15,0}, //SYM_PTCHUP
442 : {0x16,0}, //SYM_PTCHDWN
443 : {0xDC,0}, //SYM_XERR
444 : {0x59,1}, //SYM_TERALT
445 : {0x92,0}, //SYM_KN
446 : {0x85,0}, //SYM_NM
447 : {0x20,0}, //SYM_EFF
448 : {0xD3,0}, //SYM_AH
449 : {0x72,0}, //SYM_MW
450 : {0x91,1}, //SYM_FENCE_ENABLED
451 : {0x20,0}, //SYM_FENCE_DISABLED
452 : {0xBA,1}, //SYM_RNGFD
453 : {0x02,0}, //SYM_LQ
454 : {0x12,0}, //SYM_DB
455 : {0x13,0}, //SYM_DBM
456 : {0x14,0}, //SYM_SNR
457 : {0x59,0}, //SYM_ANT
458 : {0x40,1}, //SYM_ARROW_RIGHT
459 : {0x48,1}, //SYM_ARROW_LEFT
460 : {0xFF,0}
461 : //E0-FF above
462 : };
463 : #endif //AP_MSP_INAV_FONTS_ENABLED
464 :
465 : #endif
|
