From 4f69f9cc2369b6dda51b7dc39a5fab05de224859 Mon Sep 17 00:00:00 2001 From: yaapu Date: Fri, 7 Jul 2023 13:30:54 -0500 Subject: [PATCH] AP_OSD: added OSD_TYPE2 param to enable dual OSDs backend support Co-authored-by:HWurzburg(hurzburg@yahoo.com) up to 2 OSD instances can run at the same time sharing a single OSD thread ) --- libraries/AP_OSD/AP_OSD.cpp | 109 +++++++++++++++++----- libraries/AP_OSD/AP_OSD.h | 8 +- libraries/AP_OSD/AP_OSD_Backend.h | 2 + libraries/AP_OSD/AP_OSD_MAX7456.h | 20 +++- libraries/AP_OSD/AP_OSD_MSP.h | 17 ++++ libraries/AP_OSD/AP_OSD_MSP_DisplayPort.h | 17 ++++ libraries/AP_OSD/AP_OSD_ParamScreen.cpp | 13 --- libraries/AP_OSD/AP_OSD_SITL.h | 19 +++- 8 files changed, 164 insertions(+), 41 deletions(-) diff --git a/libraries/AP_OSD/AP_OSD.cpp b/libraries/AP_OSD/AP_OSD.cpp index e883c8c1de..6223e5d9f6 100644 --- a/libraries/AP_OSD/AP_OSD.cpp +++ b/libraries/AP_OSD/AP_OSD.cpp @@ -232,6 +232,15 @@ const AP_Param::GroupInfo AP_OSD::var_info[] = { AP_SUBGROUPINFO2(screen[2], "3_", 29, AP_OSD, AP_OSD_Screen), AP_SUBGROUPINFO2(screen[3], "4_", 30, AP_OSD, AP_OSD_Screen), #endif + + // @Param: _TYPE2 + // @DisplayName: OSD type 2 + // @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. + // @Values: 0:None,1:MAX7456,2:SITL,3:MSP,4:TXONLY,5:MSP_DISPLAYPORT + // @User: Standard + // @RebootRequired: True + AP_GROUPINFO("_TYPE2", 32, AP_OSD, osd_type2, 0), + AP_GROUPEND }; @@ -263,7 +272,29 @@ AP_OSD::AP_OSD() void AP_OSD::init() { - switch (osd_types(osd_type.get())) { + const AP_OSD::osd_types types[OSD_MAX_INSTANCES] = { + osd_types(osd_type.get()), + osd_types(osd_type2.get()) + }; + for (uint8_t instance = 0; instance < OSD_MAX_INSTANCES; instance++) { + if (init_backend(types[instance], instance)) { + _backend_count++; + } + } + if (_backend_count > 0) { + hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_OSD::osd_thread, void), "OSD", 1280, AP_HAL::Scheduler::PRIORITY_IO, 1); + } +} + +bool AP_OSD::init_backend(const AP_OSD::osd_types type, const uint8_t instance) +{ + // check if we can run this backend instance in parallel with backend instance 0 + if (instance > 0) { + if (_backends[0] && !_backends[0]->is_compatible_with_backend_type(type)) { + return false; + } + } + switch (type) { case OSD_NONE: case OSD_TXONLY: default: @@ -276,9 +307,9 @@ void AP_OSD::init() break; } #if HAL_WITH_OSD_BITMAP - backend = AP_OSD_MAX7456::probe(*this, std::move(spi_dev)); + _backends[instance] = AP_OSD_MAX7456::probe(*this, std::move(spi_dev)); #endif - if (backend == nullptr) { + if (_backends[instance] == nullptr) { break; } DEV_PRINTF("Started MAX7456 OSD\n"); @@ -288,8 +319,8 @@ void AP_OSD::init() #ifdef WITH_SITL_OSD case OSD_SITL: { - backend = AP_OSD_SITL::probe(*this); - if (backend == nullptr) { + _backends[instance] = AP_OSD_SITL::probe(*this); + if (_backends[instance] == nullptr) { break; } DEV_PRINTF("Started SITL OSD\n"); @@ -297,8 +328,8 @@ void AP_OSD::init() } #endif case OSD_MSP: { - backend = AP_OSD_MSP::probe(*this); - if (backend == nullptr) { + _backends[instance] = AP_OSD_MSP::probe(*this); + if (_backends[instance] == nullptr) { break; } DEV_PRINTF("Started MSP OSD\n"); @@ -306,8 +337,8 @@ void AP_OSD::init() } #if HAL_WITH_MSP_DISPLAYPORT case OSD_MSP_DISPLAYPORT: { - backend = AP_OSD_MSP_DisplayPort::probe(*this); - if (backend == nullptr) { + _backends[instance] = AP_OSD_MSP_DisplayPort::probe(*this); + if (_backends[instance] == nullptr) { break; } DEV_PRINTF("Started MSP DisplayPort OSD\n"); @@ -316,12 +347,12 @@ void AP_OSD::init() #endif } #if OSD_ENABLED - if (backend != nullptr) { + if (_backends[instance] != nullptr) { // populate the fonts lookup table - backend->init_symbol_set(AP_OSD_AbstractScreen::symbols_lookup_table, AP_OSD_NUM_SYMBOLS); - // create thread as higher priority than IO for all backends - hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_OSD::osd_thread, void), "OSD", 1280, AP_HAL::Scheduler::PRIORITY_IO, 1); + _backends[instance]->init_symbol_set(AP_OSD_AbstractScreen::symbols_lookup_table, AP_OSD_NUM_SYMBOLS); + return true; } + return false; #endif } @@ -329,30 +360,36 @@ void AP_OSD::init() void AP_OSD::osd_thread() { // initialize thread specific code once - backend->osd_thread_run_once(); + for (uint8_t instance = 0; instance < _backend_count; instance++) { + _backends[instance]->osd_thread_run_once(); + } + while (true) { hal.scheduler->delay(100); + if (!_disable) { + update_stats(); + update_current_screen(); + } update_osd(); } } void AP_OSD::update_osd() { - backend->clear(); + for (uint8_t instance = 0; instance < _backend_count; instance++) { + _backends[instance]->clear(); - if (!_disable) { - update_stats(); - update_current_screen(); - - get_screen(current_screen).set_backend(backend); - // skip drawing for MSP OSD backends to save some resources - if (osd_types(osd_type.get()) != OSD_MSP) { - get_screen(current_screen).draw(); + if (!_disable) { + get_screen(current_screen).set_backend(_backends[instance]); + // skip drawing for MSP OSD backends to save some resources + if (_backends[instance]->get_backend_type() != OSD_MSP) { + get_screen(current_screen).draw(); + } } - } - backend->flush(); + _backends[instance]->flush(); + } } //update maximums and totals @@ -545,6 +582,28 @@ void AP_OSD::set_nav_info(NavInfo &navinfo) // do this without a lock for now nav_info = navinfo; } + +// pre_arm_check - returns true if all pre-takeoff checks have completed successfully +bool AP_OSD::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const +{ +#if OSD_PARAM_ENABLED + // currently in the OSD menu, do not allow arming + if (!is_readonly_screen()) { + hal.util->snprintf(failure_msg, failure_msg_len, "In OSD menu"); + return false; + } +#endif + + //check if second backend was requested by user but not instantiated + if (osd_type.get() != OSD_NONE && _backend_count == 1 && osd_type2.get() != OSD_NONE) { + hal.util->snprintf(failure_msg, failure_msg_len, "OSD_TYPE2 not compatible with first OSD"); + return false; + } + + // if we got this far everything must be ok + return true; +} + #endif // OSD_ENABLED // handle OSD parameter configuration diff --git a/libraries/AP_OSD/AP_OSD.h b/libraries/AP_OSD/AP_OSD.h index c417234819..1dd423c483 100644 --- a/libraries/AP_OSD/AP_OSD.h +++ b/libraries/AP_OSD/AP_OSD.h @@ -49,6 +49,7 @@ class AP_MSP; #define PARAM_TOKEN_INDEX(token) PARAM_INDEX(AP_Param::get_persistent_key(token.key), token.idx, token.group_element) #define AP_OSD_NUM_SYMBOLS 91 +#define OSD_MAX_INSTANCES 2 /* class to hold one setting */ @@ -506,6 +507,9 @@ public: OSD_TXONLY=4, OSD_MSP_DISPLAYPORT=5 }; + + bool init_backend(const osd_types type, const uint8_t instance); + enum switch_method { TOGGLE=0, PWM_RANGE=1, @@ -513,6 +517,7 @@ public: }; AP_Int8 osd_type; + AP_Int8 osd_type2; // additional backend active in parallel AP_Int8 font_num; AP_Int32 options; @@ -649,7 +654,8 @@ private: StatsInfo _stats; #endif - AP_OSD_Backend *backend; + AP_OSD_Backend *_backends[OSD_MAX_INSTANCES]; + uint8_t _backend_count; static AP_OSD *_singleton; // multi-thread access support diff --git a/libraries/AP_OSD/AP_OSD_Backend.h b/libraries/AP_OSD/AP_OSD_Backend.h index 30e638f1f1..1e553b4c52 100644 --- a/libraries/AP_OSD/AP_OSD_Backend.h +++ b/libraries/AP_OSD/AP_OSD_Backend.h @@ -55,6 +55,8 @@ public: // copy the backend specific symbol set to the OSD lookup table virtual void init_symbol_set(uint8_t *symbols, const uint8_t size); + virtual bool is_compatible_with_backend_type(AP_OSD::osd_types type) const = 0; + virtual AP_OSD::osd_types get_backend_type() const = 0; // called by the OSD thread once virtual void osd_thread_run_once() { return; } diff --git a/libraries/AP_OSD/AP_OSD_MAX7456.h b/libraries/AP_OSD/AP_OSD_MAX7456.h index 5c8c5457e5..d2491c8aa6 100644 --- a/libraries/AP_OSD/AP_OSD_MAX7456.h +++ b/libraries/AP_OSD/AP_OSD_MAX7456.h @@ -39,7 +39,25 @@ public: void clear() override; // return a correction factor used to display angles correctly - float get_aspect_ratio_correction() const override; + float get_aspect_ratio_correction() const override; + + bool is_compatible_with_backend_type(AP_OSD::osd_types type) const override { + switch(type) { + case AP_OSD::osd_types::OSD_MAX7456: + case AP_OSD::osd_types::OSD_SITL: + return false; + case AP_OSD::osd_types::OSD_NONE: + case AP_OSD::osd_types::OSD_TXONLY: + case AP_OSD::osd_types::OSD_MSP: + case AP_OSD::osd_types::OSD_MSP_DISPLAYPORT: + return true; + } + return false; + } + + AP_OSD::osd_types get_backend_type() const override { + return AP_OSD::osd_types::OSD_MAX7456; + } private: diff --git a/libraries/AP_OSD/AP_OSD_MSP.h b/libraries/AP_OSD/AP_OSD_MSP.h index 984756cc3f..370d618a4c 100644 --- a/libraries/AP_OSD/AP_OSD_MSP.h +++ b/libraries/AP_OSD/AP_OSD_MSP.h @@ -19,6 +19,23 @@ public: //clear framebuffer void clear() override {}; + bool is_compatible_with_backend_type(AP_OSD::osd_types type) const override { + switch(type) { + case AP_OSD::osd_types::OSD_MSP: + case AP_OSD::osd_types::OSD_MSP_DISPLAYPORT: + return false; + case AP_OSD::osd_types::OSD_NONE: + case AP_OSD::osd_types::OSD_TXONLY: + case AP_OSD::osd_types::OSD_MAX7456: + case AP_OSD::osd_types::OSD_SITL: + return true; + } + return false; + } + + AP_OSD::osd_types get_backend_type() const override { + return AP_OSD::osd_types::OSD_MSP; + } private: void setup_defaults(void); }; diff --git a/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.h b/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.h index 2cc81eba27..fa02451d4d 100644 --- a/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.h +++ b/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.h @@ -30,7 +30,24 @@ public: // return a correction factor used to display angles correctly float get_aspect_ratio_correction() const override; + + bool is_compatible_with_backend_type(AP_OSD::osd_types type) const override { + switch(type) { + case AP_OSD::osd_types::OSD_MSP: + case AP_OSD::osd_types::OSD_MSP_DISPLAYPORT: + return false; + case AP_OSD::osd_types::OSD_NONE: + case AP_OSD::osd_types::OSD_TXONLY: + case AP_OSD::osd_types::OSD_MAX7456: + case AP_OSD::osd_types::OSD_SITL: + return true; + } + return false; + } + AP_OSD::osd_types get_backend_type() const override { + return AP_OSD::osd_types::OSD_MSP_DISPLAYPORT; + } protected: uint8_t format_string_for_osd(char* dst, uint8_t size, bool decimal_packed, const char *fmt, va_list ap) override; diff --git a/libraries/AP_OSD/AP_OSD_ParamScreen.cpp b/libraries/AP_OSD/AP_OSD_ParamScreen.cpp index 938e7cd29d..f61e2d8712 100644 --- a/libraries/AP_OSD/AP_OSD_ParamScreen.cpp +++ b/libraries/AP_OSD/AP_OSD_ParamScreen.cpp @@ -595,19 +595,6 @@ void AP_OSD_ParamScreen::draw(void) } #endif -// pre_arm_check - returns true if all pre-takeoff checks have completed successfully -bool AP_OSD::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const -{ - // currently in the OSD menu, do not allow arming - if (!is_readonly_screen()) { - hal.util->snprintf(failure_msg, failure_msg_len, "In OSD menu"); - return false; - } - - // if we got this far everything must be ok - return true; -} - #endif // OSD_ENABLED // save all of the parameters diff --git a/libraries/AP_OSD/AP_OSD_SITL.h b/libraries/AP_OSD/AP_OSD_SITL.h index 50b068b25d..dd97deba20 100644 --- a/libraries/AP_OSD/AP_OSD_SITL.h +++ b/libraries/AP_OSD/AP_OSD_SITL.h @@ -34,7 +34,7 @@ public: //draw given text to framebuffer void write(uint8_t x, uint8_t y, const char* text) override; - //initilize display port and underlying hardware + //initialize display port and underlying hardware bool init() override; //flush framebuffer to screen @@ -43,6 +43,23 @@ public: //clear framebuffer void clear() override; + bool is_compatible_with_backend_type(AP_OSD::osd_types type) const override { + switch(type) { + case AP_OSD::osd_types::OSD_MAX7456: + case AP_OSD::osd_types::OSD_SITL: + return false; + case AP_OSD::osd_types::OSD_NONE: + case AP_OSD::osd_types::OSD_TXONLY: + case AP_OSD::osd_types::OSD_MSP: + case AP_OSD::osd_types::OSD_MSP_DISPLAYPORT: + return true; + } + return false; + } + + AP_OSD::osd_types get_backend_type() const override { + return AP_OSD::osd_types::OSD_SITL; + } private: //constructor AP_OSD_SITL(AP_OSD &osd);