diff --git a/libraries/AP_OSD/AP_OSD.cpp b/libraries/AP_OSD/AP_OSD.cpp index 3e3bd3fe90..8bfa934e61 100644 --- a/libraries/AP_OSD/AP_OSD.cpp +++ b/libraries/AP_OSD/AP_OSD.cpp @@ -109,7 +109,6 @@ void AP_OSD::init() break; } hal.console->printf("Started MAX7456 OSD\n"); - hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_OSD::timer, void)); break; } @@ -120,19 +119,20 @@ void AP_OSD::init() break; } hal.console->printf("Started SITL OSD\n"); - hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_OSD::timer, void)); break; } #endif } + if (backend != nullptr) { + // create thread as higher priority than IO + hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_OSD::osd_thread, void), "OSD", 512, AP_HAL::Scheduler::PRIORITY_IO, 1); + } } -void AP_OSD::timer() +void AP_OSD::osd_thread() { - uint32_t now = AP_HAL::millis(); - - if (now - last_update_ms >= 100) { - last_update_ms = now; + while (true) { + hal.scheduler->delay(100); update_osd(); } } diff --git a/libraries/AP_OSD/AP_OSD.h b/libraries/AP_OSD/AP_OSD.h index 91c7f874dc..959a84db88 100644 --- a/libraries/AP_OSD/AP_OSD.h +++ b/libraries/AP_OSD/AP_OSD.h @@ -171,7 +171,7 @@ public: AP_OSD_Screen screen[AP_OSD_NUM_SCREENS]; private: - void timer(); + void osd_thread(); void update_osd(); void update_current_screen(); void next_screen();