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 : * AP_OSD partially based on betaflight and inav osd.c implemention.
16 : * clarity.mcm font is taken from inav configurator.
17 : * Many thanks to their authors.
18 : */
19 :
20 : #include "AP_OSD.h"
21 :
22 : #if OSD_ENABLED || OSD_PARAM_ENABLED
23 :
24 : #include "AP_OSD_MAX7456.h"
25 : #ifdef WITH_SITL_OSD
26 : #include "AP_OSD_SITL.h"
27 : #endif
28 : #include "AP_OSD_MSP.h"
29 : #include "AP_OSD_MSP_DisplayPort.h"
30 : #include <AP_HAL/AP_HAL.h>
31 : #include <AP_HAL/Util.h>
32 : #include <RC_Channel/RC_Channel.h>
33 : #include <AP_AHRS/AP_AHRS.h>
34 : #include <AP_BattMonitor/AP_BattMonitor.h>
35 : #include <utility>
36 : #include <AP_Notify/AP_Notify.h>
37 : #include <AP_Terrain/AP_Terrain.h>
38 : #include <AP_RSSI/AP_RSSI.h>
39 : #include <GCS_MAVLink/GCS.h>
40 :
41 : // macro for easy use of var_info2
42 : #define AP_SUBGROUPINFO2(element, name, idx, thisclazz, elclazz) { name, AP_VAROFFSET(thisclazz, element), { group_info : elclazz::var_info2 }, AP_PARAM_FLAG_NESTED_OFFSET, idx, AP_PARAM_GROUP }
43 :
44 : const AP_Param::GroupInfo AP_OSD::var_info[] = {
45 :
46 : // @Param: _TYPE
47 : // @DisplayName: OSD type
48 : // @Description: OSD type. TXONLY makes the OSD parameter selection available to other modules even if there is no native OSD support on the board, for instance CRSF.
49 : // @Values: 0:None,1:MAX7456,2:SITL,3:MSP,4:TXONLY,5:MSP_DISPLAYPORT
50 : // @User: Standard
51 : // @RebootRequired: True
52 : AP_GROUPINFO_FLAGS("_TYPE", 1, AP_OSD, osd_type, 0, AP_PARAM_FLAG_ENABLE),
53 :
54 : #if OSD_ENABLED
55 : // @Param: _CHAN
56 : // @DisplayName: Screen switch transmitter channel
57 : // @Description: This sets the channel used to switch different OSD screens.
58 : // @Values: 0:Disable,5:Chan5,6:Chan6,7:Chan7,8:Chan8,9:Chan9,10:Chan10,11:Chan11,12:Chan12,13:Chan13,14:Chan14,15:Chan15,16:Chan16
59 : // @User: Standard
60 : AP_GROUPINFO("_CHAN", 2, AP_OSD, rc_channel, 0),
61 :
62 : // @Group: 1_
63 : // @Path: AP_OSD_Screen.cpp
64 : AP_SUBGROUPINFO(screen[0], "1_", 3, AP_OSD, AP_OSD_Screen),
65 :
66 : // @Group: 2_
67 : // @Path: AP_OSD_Screen.cpp
68 : AP_SUBGROUPINFO(screen[1], "2_", 4, AP_OSD, AP_OSD_Screen),
69 :
70 : // @Group: 3_
71 : // @Path: AP_OSD_Screen.cpp
72 : AP_SUBGROUPINFO(screen[2], "3_", 5, AP_OSD, AP_OSD_Screen),
73 :
74 : // @Group: 4_
75 : // @Path: AP_OSD_Screen.cpp
76 : AP_SUBGROUPINFO(screen[3], "4_", 6, AP_OSD, AP_OSD_Screen),
77 :
78 : // @Param: _SW_METHOD
79 : // @DisplayName: Screen switch method
80 : // @Description: This sets the method used to switch different OSD screens.
81 : // @Values: 0: switch to next screen if channel value was changed, 1: select screen based on pwm ranges specified for each screen, 2: switch to next screen after low to high transition and every 1s while channel value is high
82 : // @User: Standard
83 : AP_GROUPINFO("_SW_METHOD", 7, AP_OSD, sw_method, AP_OSD::TOGGLE),
84 :
85 : // @Param: _OPTIONS
86 : // @DisplayName: OSD Options
87 : // @Description: This sets options that change the display
88 : // @Bitmask: 0:UseDecimalPack, 1:InvertedWindArrow, 2:InvertedAHRoll, 3:Convert feet to miles at 5280ft instead of 10000ft, 4:DisableCrosshair, 5:TranslateArrows, 6:AviationStyleAH, 7:Prefix LQ with RF Mode
89 : // @User: Standard
90 : AP_GROUPINFO("_OPTIONS", 8, AP_OSD, options, OPTION_DECIMAL_PACK),
91 :
92 : // @Param: _FONT
93 : // @DisplayName: OSD Font
94 : // @Description: This sets which OSD font to use. It is an integer from 0 to the number of fonts available
95 : // @User: Standard
96 : // @RebootRequired: True
97 : AP_GROUPINFO("_FONT", 9, AP_OSD, font_num, 0),
98 :
99 : // @Param: _V_OFFSET
100 : // @DisplayName: OSD vertical offset
101 : // @Description: Sets vertical offset of the osd inside image
102 : // @Range: 0 31
103 : // @User: Standard
104 : // @RebootRequired: True
105 : AP_GROUPINFO("_V_OFFSET", 10, AP_OSD, v_offset, 16),
106 :
107 : // @Param: _H_OFFSET
108 : // @DisplayName: OSD horizontal offset
109 : // @Description: Sets horizontal offset of the osd inside image
110 : // @Range: 0 63
111 : // @User: Standard
112 : // @RebootRequired: True
113 : AP_GROUPINFO("_H_OFFSET", 11, AP_OSD, h_offset, 32),
114 :
115 : // @Param: _W_RSSI
116 : // @DisplayName: RSSI warn level (in %)
117 : // @Description: Set level at which RSSI item will flash (in positive % or negative dBm values as applicable). 30% or -100dBm are defaults.
118 : // @Range: -128 100
119 : // @User: Standard
120 : AP_GROUPINFO("_W_RSSI", 12, AP_OSD, warn_rssi, AP_OSD_WARN_RSSI_DEFAULT),
121 :
122 : // @Param: _W_NSAT
123 : // @DisplayName: NSAT warn level
124 : // @Description: Set level at which NSAT item will flash
125 : // @Range: 1 30
126 : // @User: Standard
127 : AP_GROUPINFO("_W_NSAT", 13, AP_OSD, warn_nsat, 9),
128 :
129 : // @Param: _W_BATVOLT
130 : // @DisplayName: BAT_VOLT warn level
131 : // @Description: Set level at which BAT_VOLT item will flash
132 : // @Range: 0 100
133 : // @User: Standard
134 : AP_GROUPINFO("_W_BATVOLT", 14, AP_OSD, warn_batvolt, 10.0f),
135 :
136 : // @Param: _UNITS
137 : // @DisplayName: Display Units
138 : // @Description: Sets the units to use in displaying items
139 : // @Values: 0:Metric,1:Imperial,2:SI,3:Aviation
140 : // @User: Standard
141 : AP_GROUPINFO("_UNITS", 15, AP_OSD, units, 0),
142 :
143 : // @Param: _MSG_TIME
144 : // @DisplayName: Message display duration in seconds
145 : // @Description: Sets message duration seconds
146 : // @Range: 1 20
147 : // @User: Standard
148 : AP_GROUPINFO("_MSG_TIME", 16, AP_OSD, msgtime_s, 10),
149 :
150 : // @Param: _ARM_SCR
151 : // @DisplayName: Arm screen
152 : // @Description: Screen to be shown on Arm event. Zero to disable the feature.
153 : // @Range: 0 4
154 : // @User: Standard
155 : AP_GROUPINFO("_ARM_SCR", 17, AP_OSD, arm_scr, 0),
156 :
157 : // @Param: _DSARM_SCR
158 : // @DisplayName: Disarm screen
159 : // @Description: Screen to be shown on disarm event. Zero to disable the feature.
160 : // @Range: 0 4
161 : // @User: Standard
162 : AP_GROUPINFO("_DSARM_SCR", 18, AP_OSD, disarm_scr, 0),
163 :
164 : // @Param: _FS_SCR
165 : // @DisplayName: Failsafe screen
166 : // @Description: Screen to be shown on failsafe event. Zero to disable the feature.
167 : // @Range: 0 4
168 : // @User: Standard
169 : AP_GROUPINFO("_FS_SCR", 19, AP_OSD, failsafe_scr, 0),
170 :
171 : #if OSD_PARAM_ENABLED
172 : // @Param: _BTN_DELAY
173 : // @DisplayName: Button delay
174 : // @Description: Debounce time in ms for stick commanded parameter navigation.
175 : // @Range: 0 3000
176 : // @User: Advanced
177 : AP_GROUPINFO("_BTN_DELAY", 20, AP_OSD, button_delay_ms, 300),
178 : #endif
179 : #if AP_TERRAIN_AVAILABLE
180 : // @Param: _W_TERR
181 : // @DisplayName: Terrain warn level
182 : // @Description: Set level below which TER_HGT item will flash. -1 disables.
183 : // @Range: -1 3000
184 : // @Units: m
185 : // @User: Standard
186 : AP_GROUPINFO("_W_TERR", 23, AP_OSD, warn_terr, -1),
187 : #endif
188 :
189 : // @Param: _W_AVGCELLV
190 : // @DisplayName: AVGCELLV warn level
191 : // @Description: Set level at which AVGCELLV item will flash
192 : // @Range: 0 100
193 : // @User: Standard
194 : AP_GROUPINFO("_W_AVGCELLV", 24, AP_OSD, warn_avgcellvolt, 3.6f),
195 :
196 : // @Param: _CELL_COUNT
197 : // @DisplayName: Battery cell count
198 : // @Description: Used for average cell voltage display. -1 disables, 0 uses cell count autodetection for well charged LIPO/LIION batteries at connection, other values manually select cell count used.
199 : // @Increment: 1
200 : // @User: Advanced
201 : AP_GROUPINFO("_CELL_COUNT", 25, AP_OSD, cell_count, -1),
202 :
203 : // @Param: _W_RESTVOLT
204 : // @DisplayName: RESTVOLT warn level
205 : // @Description: Set level at which RESTVOLT item will flash
206 : // @Range: 0 100
207 : // @User: Standard
208 : AP_GROUPINFO("_W_RESTVOLT", 26, AP_OSD, warn_restvolt, 10.0f),
209 :
210 : // @Param: _W_ACRVOLT
211 : // @DisplayName: Avg Cell Resting Volt warn level
212 : // @Description: Set level at which ACRVOLT item will flash
213 : // @Range: 0 100
214 : // @User: Standard
215 : AP_GROUPINFO("_W_ACRVOLT", 31, AP_OSD, warn_avgcellrestvolt, 3.6f),
216 :
217 : #if AP_OSD_EXTENDED_LNK_STATS
218 : // @Param: _W_LQ
219 : // @DisplayName: RC link quality warn level (in %)
220 : // @Description: Set level at which RC_LQ item will flash (%)
221 : // @Range: 0 100
222 : // @User: Standard
223 : AP_GROUPINFO("_W_LQ", 33, AP_OSD, warn_lq, 50),
224 :
225 : // @Param: _W_SNR
226 : // @DisplayName: RC link SNR warn level (in %)
227 : // @Description: Set level at which RC_SNR item will flash (in db)
228 : // @Range: -20 10
229 : // @User: Standard
230 : AP_GROUPINFO("_W_SNR", 34, AP_OSD, warn_snr, 0),
231 : #endif
232 :
233 : #if HAL_OSD_SIDEBAR_ENABLE
234 : // @Param: _SB_H_OFS
235 : // @DisplayName: Sidebar horizontal offset
236 : // @Description: Extends the spacing between the sidebar elements by this amount of columns. Positive values increases the width to the right of the screen.
237 : // @Range: 0 20
238 : // @User: Standard
239 : AP_GROUPINFO("_SB_H_OFS", 35, AP_OSD, sidebar_h_offset, 0),
240 :
241 : // @Param: _SB_V_EXT
242 : // @DisplayName: Sidebar vertical extension
243 : // @Description: Increase of vertical length of the sidebar itens by this amount of lines. Applied equally both above and below the default setting.
244 : // @Range: 0 10
245 : // @User: Standard
246 : AP_GROUPINFO("_SB_V_EXT", 36, AP_OSD, sidebar_v_ext, 0),
247 : #endif // HAL_OSD_SIDEBAR_ENABLE
248 :
249 : #endif //osd enabled
250 : #if OSD_PARAM_ENABLED
251 : // @Group: 5_
252 : // @Path: AP_OSD_ParamScreen.cpp
253 : AP_SUBGROUPINFO(param_screen[0], "5_", 21, AP_OSD, AP_OSD_ParamScreen),
254 :
255 : // @Group: 6_
256 : // @Path: AP_OSD_ParamScreen.cpp
257 : AP_SUBGROUPINFO(param_screen[1], "6_", 22, AP_OSD, AP_OSD_ParamScreen),
258 : #endif
259 :
260 : #if OSD_ENABLED
261 : // additional tables to go beyond 63 limit
262 : AP_SUBGROUPINFO2(screen[0], "1_", 27, AP_OSD, AP_OSD_Screen),
263 : AP_SUBGROUPINFO2(screen[1], "2_", 28, AP_OSD, AP_OSD_Screen),
264 : AP_SUBGROUPINFO2(screen[2], "3_", 29, AP_OSD, AP_OSD_Screen),
265 : AP_SUBGROUPINFO2(screen[3], "4_", 30, AP_OSD, AP_OSD_Screen),
266 : #endif
267 :
268 : // @Param: _TYPE2
269 : // @DisplayName: OSD type 2
270 : // @Description: OSD type 2. TXONLY makes the OSD parameter selection available to other modules even if there is no native OSD support on the board, for instance CRSF.
271 : // @Values: 0:None,1:MAX7456,2:SITL,3:MSP,4:TXONLY,5:MSP_DISPLAYPORT
272 : // @User: Standard
273 : // @RebootRequired: True
274 : AP_GROUPINFO("_TYPE2", 32, AP_OSD, osd_type2, 0),
275 :
276 : AP_GROUPEND
277 : };
278 :
279 : extern const AP_HAL::HAL& hal;
280 :
281 : // singleton instance
282 : AP_OSD *AP_OSD::_singleton;
283 :
284 10305 : AP_OSD::AP_OSD()
285 : {
286 2061 : if (_singleton != nullptr) {
287 0 : AP_HAL::panic("AP_OSD must be singleton");
288 : }
289 2061 : AP_Param::setup_object_defaults(this, var_info);
290 : #if OSD_ENABLED
291 : // force first screen enabled
292 2061 : screen[0].enabled.set_and_default(1);
293 2061 : previous_pwm_screen = -1;
294 : #endif
295 : #ifdef WITH_SITL_OSD
296 : osd_type.set_default(OSD_SITL);
297 : #endif
298 :
299 : #ifdef HAL_OSD_TYPE_DEFAULT
300 : osd_type.set_default(HAL_OSD_TYPE_DEFAULT);
301 : #endif
302 2061 : _singleton = this;
303 2061 : }
304 :
305 2061 : void AP_OSD::init()
306 : {
307 : const AP_OSD::osd_types types[OSD_MAX_INSTANCES] = {
308 2061 : osd_types(osd_type.get()),
309 4122 : osd_types(osd_type2.get())
310 2061 : };
311 6183 : for (uint8_t instance = 0; instance < OSD_MAX_INSTANCES; instance++) {
312 4122 : if (init_backend(types[instance], instance)) {
313 2 : _backend_count++;
314 : }
315 : }
316 2061 : if (_backend_count > 0) {
317 2 : hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_OSD::osd_thread, void), "OSD", 1280, AP_HAL::Scheduler::PRIORITY_IO, 1);
318 : }
319 2061 : }
320 :
321 4122 : bool AP_OSD::init_backend(const AP_OSD::osd_types type, const uint8_t instance)
322 : {
323 : // don't re-initialize if backend already exists
324 4122 : if (_backends[instance] != nullptr) {
325 0 : return false;
326 : }
327 :
328 : // check if we can run this backend instance in parallel with backend instance 0
329 4122 : if (instance > 0) {
330 2061 : if (_backends[0] && !_backends[0]->is_compatible_with_backend_type(type)) {
331 0 : return false;
332 : }
333 : }
334 4122 : switch (type) {
335 4120 : case OSD_NONE:
336 : case OSD_TXONLY:
337 : default:
338 4120 : break;
339 :
340 0 : case OSD_MAX7456: {
341 : #ifdef HAL_WITH_SPI_OSD
342 : AP_HAL::OwnPtr<AP_HAL::Device> spi_dev = std::move(hal.spi->get_device("osd"));
343 : if (!spi_dev) {
344 : break;
345 : }
346 : #if HAL_WITH_OSD_BITMAP
347 : _backends[instance] = AP_OSD_MAX7456::probe(*this, std::move(spi_dev));
348 : #endif
349 : if (_backends[instance] == nullptr) {
350 : break;
351 : }
352 : DEV_PRINTF("Started MAX7456 OSD\n");
353 : #endif
354 0 : break;
355 : }
356 :
357 : #ifdef WITH_SITL_OSD
358 : case OSD_SITL: {
359 : _backends[instance] = AP_OSD_SITL::probe(*this);
360 : if (_backends[instance] == nullptr) {
361 : break;
362 : }
363 : DEV_PRINTF("Started SITL OSD\n");
364 : break;
365 : }
366 : #endif
367 0 : case OSD_MSP: {
368 0 : _backends[instance] = AP_OSD_MSP::probe(*this);
369 0 : if (_backends[instance] == nullptr) {
370 0 : break;
371 : }
372 0 : DEV_PRINTF("Started MSP OSD\n");
373 0 : break;
374 : }
375 : #if HAL_WITH_MSP_DISPLAYPORT
376 2 : case OSD_MSP_DISPLAYPORT: {
377 2 : _backends[instance] = AP_OSD_MSP_DisplayPort::probe(*this);
378 2 : if (_backends[instance] == nullptr) {
379 0 : break;
380 : }
381 2 : DEV_PRINTF("Started MSP DisplayPort OSD\n");
382 2 : break;
383 : }
384 : #endif
385 : }
386 : #if OSD_ENABLED
387 4122 : if (_backends[instance] != nullptr) {
388 : // populate the fonts lookup table
389 2 : _backends[instance]->init_symbol_set(AP_OSD_AbstractScreen::symbols_lookup_table, AP_OSD_NUM_SYMBOLS);
390 2 : return true;
391 : }
392 : #endif
393 4120 : return false;
394 : }
395 :
396 : #if OSD_ENABLED
397 2 : void AP_OSD::osd_thread()
398 : {
399 : // initialize thread specific code once
400 4 : for (uint8_t instance = 0; instance < _backend_count; instance++) {
401 2 : _backends[instance]->osd_thread_run_once();
402 : }
403 :
404 :
405 : while (true) {
406 550 : hal.scheduler->delay(100);
407 548 : if (!_disable) {
408 548 : update_stats();
409 548 : update_current_screen();
410 : }
411 : #if AP_SCRIPTING_ENABLED
412 : // hold the semaphore so scripting cannot draw to the OSD while the
413 : // OSD thread is updating it, and vice versa
414 548 : WITH_SEMAPHORE(_sem);
415 548 : if (!scripting_override) {
416 548 : update_osd();
417 : } else {
418 0 : override_count++;
419 0 : if (override_count > 20) {
420 : // timeout after 2 seconds and return to normal OSD
421 0 : scripting_override = false;
422 0 : override_count = 0;
423 : }
424 : }
425 : #else
426 : update_osd();
427 : #endif // AP_SCRIPTING_ENABLED
428 548 : }
429 : }
430 :
431 548 : void AP_OSD::update_osd()
432 : {
433 1096 : for (uint8_t instance = 0; instance < _backend_count; instance++) {
434 548 : _backends[instance]->clear();
435 :
436 548 : if (!_disable) {
437 548 : get_screen(current_screen).set_backend(_backends[instance]);
438 : // skip drawing for MSP OSD backends to save some resources
439 548 : if (_backends[instance]->get_backend_type() != OSD_MSP) {
440 548 : get_screen(current_screen).draw();
441 : }
442 : }
443 :
444 548 : _backends[instance]->flush();
445 : }
446 548 : }
447 :
448 : //update maximums and totals
449 548 : void AP_OSD::update_stats()
450 : {
451 : // allow other threads to consume stats info
452 548 : WITH_SEMAPHORE(_sem);
453 :
454 548 : uint32_t now = AP_HAL::millis();
455 548 : if (!AP_Notify::flags.armed) {
456 548 : _stats.last_update_ms = now;
457 548 : return;
458 : }
459 :
460 : // flight distance
461 0 : uint32_t delta_ms = now - _stats.last_update_ms;
462 0 : _stats.last_update_ms = now;
463 :
464 0 : Vector2f v;
465 0 : Location loc {};
466 0 : Location home_loc;
467 : bool home_is_set;
468 : bool have_airspeed_estimate;
469 : float alt;
470 0 : float aspd_mps = 0.0f;
471 : {
472 : // minimize semaphore scope
473 0 : AP_AHRS &ahrs = AP::ahrs();
474 0 : WITH_SEMAPHORE(ahrs.get_semaphore());
475 0 : v = ahrs.groundspeed_vector();
476 0 : home_is_set = ahrs.get_location(loc) && ahrs.home_is_set();
477 0 : if (home_is_set) {
478 0 : home_loc = ahrs.get_home();
479 : }
480 0 : ahrs.get_relative_position_D_home(alt);
481 0 : have_airspeed_estimate = ahrs.airspeed_EAS(aspd_mps);
482 0 : }
483 0 : float speed = v.length();
484 0 : if (speed < 0.178) {
485 0 : speed = 0.0;
486 : }
487 0 : float dist_m = (speed * delta_ms)*0.001;
488 0 : _stats.last_distance_m += dist_m;
489 :
490 : // maximum ground speed
491 0 : _stats.max_speed_mps = fmaxf(_stats.max_speed_mps,speed);
492 :
493 : // maximum distance
494 0 : if (home_is_set) {
495 0 : float distance = home_loc.get_distance(loc);
496 0 : _stats.max_dist_m = fmaxf(_stats.max_dist_m, distance);
497 : }
498 :
499 : // maximum altitude
500 0 : alt = -alt;
501 0 : _stats.max_alt_m = fmaxf(_stats.max_alt_m, alt);
502 : #if AP_BATTERY_ENABLED
503 : // maximum current
504 0 : AP_BattMonitor &battery = AP::battery();
505 : float amps;
506 0 : if (battery.current_amps(amps)) {
507 0 : _stats.max_current_a = fmaxf(_stats.max_current_a, amps);
508 : }
509 : // minimum voltage
510 0 : float voltage = battery.voltage();
511 0 : if (voltage > 0) {
512 0 : _stats.min_voltage_v = fminf(_stats.min_voltage_v, voltage);
513 : }
514 : #endif
515 : #if AP_RSSI_ENABLED
516 : // minimum rssi
517 0 : AP_RSSI *ap_rssi = AP_RSSI::get_singleton();
518 0 : if (ap_rssi) {
519 0 : _stats.min_rssi = fminf(_stats.min_rssi, ap_rssi->read_receiver_rssi());
520 : }
521 : #endif
522 : // max airspeed either true or synthetic
523 0 : if (have_airspeed_estimate) {
524 0 : _stats.max_airspeed_mps = fmaxf(_stats.max_airspeed_mps, aspd_mps);
525 : }
526 : #if HAL_WITH_ESC_TELEM
527 : // max esc temp
528 0 : AP_ESC_Telem& telem = AP::esc_telem();
529 0 : int16_t highest_temperature = 0;
530 0 : telem.get_highest_temperature(highest_temperature);
531 0 : _stats.max_esc_temp = MAX(_stats.max_esc_temp, highest_temperature);
532 : #endif
533 548 : }
534 :
535 : //Thanks to minimosd authors for the multiple osd screen idea
536 548 : void AP_OSD::update_current_screen()
537 : {
538 : // Switch on ARM/DISARM event
539 548 : if (AP_Notify::flags.armed) {
540 0 : if (!was_armed && arm_scr > 0 && arm_scr <= AP_OSD_NUM_DISPLAY_SCREENS && get_screen(arm_scr-1).enabled) {
541 0 : current_screen = arm_scr-1;
542 : }
543 0 : was_armed = true;
544 548 : } else if (was_armed) {
545 0 : if (disarm_scr > 0 && disarm_scr <= AP_OSD_NUM_DISPLAY_SCREENS && get_screen(disarm_scr-1).enabled) {
546 0 : current_screen = disarm_scr-1;
547 : }
548 0 : was_armed = false;
549 : }
550 :
551 : // Switch on failsafe event
552 548 : if (AP_Notify::flags.failsafe_radio || AP_Notify::flags.failsafe_battery) {
553 0 : if (!was_failsafe && failsafe_scr > 0 && failsafe_scr <= AP_OSD_NUM_DISPLAY_SCREENS && get_screen(failsafe_scr-1).enabled) {
554 0 : pre_fs_screen = current_screen;
555 0 : current_screen = failsafe_scr-1;
556 : }
557 0 : was_failsafe = true;
558 548 : } else if (was_failsafe) {
559 0 : if (get_screen(pre_fs_screen).enabled) {
560 0 : current_screen = pre_fs_screen;
561 : }
562 0 : was_failsafe = false;
563 : }
564 :
565 548 : if (rc_channel == 0) {
566 548 : return;
567 : }
568 :
569 : #if AP_RC_CHANNEL_ENABLED
570 0 : RC_Channel *channel = rc().channel(rc_channel-1);
571 0 : if (channel == nullptr) {
572 0 : return;
573 : }
574 :
575 0 : int16_t channel_value = channel->get_radio_in();
576 0 : switch (sw_method) {
577 : //switch to next screen if channel value was changed
578 0 : default:
579 : case TOGGLE:
580 0 : if (previous_channel_value == 0) {
581 : //do not switch to the next screen just after initialization
582 0 : previous_channel_value = channel_value;
583 : }
584 0 : if (abs(channel_value-previous_channel_value) > 200) {
585 0 : if (switch_debouncer) {
586 0 : next_screen();
587 0 : previous_channel_value = channel_value;
588 : } else {
589 0 : switch_debouncer = true;
590 0 : return;
591 : }
592 : }
593 0 : break;
594 : //select screen based on pwm ranges specified
595 0 : case PWM_RANGE:
596 0 : for (int i=0; i<AP_OSD_NUM_SCREENS; i++) {
597 0 : if (get_screen(i).enabled && get_screen(i).channel_min <= channel_value && get_screen(i).channel_max > channel_value) {
598 0 : if (previous_pwm_screen == i) {
599 0 : break;
600 : } else {
601 0 : current_screen = previous_pwm_screen = i;
602 : }
603 : }
604 : }
605 0 : break;
606 : //switch to next screen after low to high transition and every 1s while channel value is high
607 0 : case AUTO_SWITCH:
608 0 : if (channel_value > channel->get_radio_trim()) {
609 0 : if (switch_debouncer) {
610 0 : uint32_t now = AP_HAL::millis();
611 0 : if (now - last_switch_ms > 1000) {
612 0 : next_screen();
613 0 : last_switch_ms = now;
614 : }
615 : } else {
616 0 : switch_debouncer = true;
617 0 : return;
618 : }
619 : } else {
620 0 : last_switch_ms = 0;
621 : }
622 0 : break;
623 : }
624 0 : switch_debouncer = false;
625 : #endif // AP_RC_CHANNEL_ENABLED
626 : }
627 :
628 : //select next avaliable screen, do nothing if all screens disabled
629 0 : void AP_OSD::next_screen()
630 : {
631 0 : uint8_t t = current_screen;
632 : do {
633 0 : t = (t + 1)%AP_OSD_NUM_SCREENS;
634 0 : } while (t != current_screen && !get_screen(t).enabled);
635 0 : current_screen = t;
636 0 : }
637 :
638 : // set navigation information for display
639 132557 : void AP_OSD::set_nav_info(NavInfo &navinfo)
640 : {
641 : // do this without a lock for now
642 132557 : nav_info = navinfo;
643 132557 : }
644 :
645 : // pre_arm_check - returns true if all pre-takeoff checks have completed successfully
646 131378 : bool AP_OSD::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const
647 : {
648 : #if OSD_PARAM_ENABLED
649 : // currently in the OSD menu, do not allow arming
650 131378 : if (!is_readonly_screen()) {
651 0 : hal.util->snprintf(failure_msg, failure_msg_len, "In OSD menu");
652 0 : return false;
653 : }
654 : #endif
655 :
656 : //check if second backend was requested by user but not instantiated
657 131378 : if (osd_type.get() != OSD_NONE && _backend_count == 1 && osd_type2.get() != OSD_NONE) {
658 0 : hal.util->snprintf(failure_msg, failure_msg_len, "OSD_TYPE2 not compatible with first OSD");
659 0 : return false;
660 : }
661 :
662 : // if we got this far everything must be ok
663 131378 : return true;
664 : }
665 :
666 : #endif // OSD_ENABLED
667 :
668 : // handle OSD parameter configuration
669 : #if HAL_GCS_ENABLED
670 0 : void AP_OSD::handle_msg(const mavlink_message_t &msg, const GCS_MAVLINK& link)
671 : {
672 0 : bool found = false;
673 :
674 0 : switch (msg.msgid) {
675 0 : case MAVLINK_MSG_ID_OSD_PARAM_CONFIG: {
676 : mavlink_osd_param_config_t packet;
677 0 : mavlink_msg_osd_param_config_decode(&msg, &packet);
678 : #if OSD_PARAM_ENABLED
679 0 : for (uint8_t i = 0; i < AP_OSD_NUM_PARAM_SCREENS; i++) {
680 0 : if (packet.osd_screen == i + AP_OSD_NUM_DISPLAY_SCREENS + 1) {
681 0 : param_screen[i].handle_write_msg(packet, link);
682 0 : found = true;
683 : }
684 : }
685 : #endif
686 : // send back an error
687 0 : if (!found) {
688 0 : mavlink_msg_osd_param_config_reply_send(link.get_chan(), packet.request_id, OSD_PARAM_INVALID_SCREEN);
689 : }
690 : }
691 0 : break;
692 0 : case MAVLINK_MSG_ID_OSD_PARAM_SHOW_CONFIG: {
693 : mavlink_osd_param_show_config_t packet;
694 0 : mavlink_msg_osd_param_show_config_decode(&msg, &packet);
695 : #if OSD_PARAM_ENABLED
696 0 : for (uint8_t i = 0; i < AP_OSD_NUM_PARAM_SCREENS; i++) {
697 0 : if (packet.osd_screen == i + AP_OSD_NUM_DISPLAY_SCREENS + 1) {
698 0 : param_screen[i].handle_read_msg(packet, link);
699 0 : found = true;
700 : }
701 : }
702 : #endif
703 : // send back an error
704 0 : if (!found) {
705 0 : mavlink_msg_osd_param_show_config_reply_send(link.get_chan(), packet.request_id, OSD_PARAM_INVALID_SCREEN,
706 : nullptr, OSD_PARAM_NONE, 0, 0, 0);
707 : }
708 : }
709 0 : break;
710 0 : default:
711 0 : break;
712 : }
713 0 : }
714 : #endif
715 :
716 310843 : AP_OSD *AP::osd() {
717 310843 : return AP_OSD::get_singleton();
718 : }
719 :
720 : #endif // OSD_ENABLED || OSD_PARAM_ENABLED
|
