diff --git a/libraries/AP_OSD/AP_OSD_SITL.cpp b/libraries/AP_OSD/AP_OSD_SITL.cpp index eac2a34067..fd03fe754c 100644 --- a/libraries/AP_OSD/AP_OSD_SITL.cpp +++ b/libraries/AP_OSD/AP_OSD_SITL.cpp @@ -103,21 +103,20 @@ void AP_OSD_SITL::write(uint8_t x, uint8_t y, const char* text) if (y >= video_lines || text == nullptr) { return; } - mutex->take_blocking(); + WITH_SEMAPHORE(mutex); + while ((x < video_cols) && (*text != 0)) { buffer[y][x] = *text; ++text; ++x; } - mutex->give(); } void AP_OSD_SITL::clear(void) { AP_OSD_Backend::clear(); - mutex->take_blocking(); + WITH_SEMAPHORE(mutex); memset(buffer, 0, sizeof(buffer)); - mutex->give(); } void AP_OSD_SITL::flush(void) @@ -150,9 +149,10 @@ void AP_OSD_SITL::update_thread(void) last_counter = counter; uint8_t buffer2[video_lines][video_cols]; - mutex->take_blocking(); - memcpy(buffer2, buffer, sizeof(buffer2)); - mutex->give(); + { + WITH_SEMAPHORE(mutex); + memcpy(buffer2, buffer, sizeof(buffer2)); + } w->clear(); for (uint8_t y=0; ynew_semaphore(); pthread_create(&thread, NULL, update_thread_start, this); return true; } diff --git a/libraries/AP_OSD/AP_OSD_SITL.h b/libraries/AP_OSD/AP_OSD_SITL.h index c15b038331..b5a0d59085 100644 --- a/libraries/AP_OSD/AP_OSD_SITL.h +++ b/libraries/AP_OSD/AP_OSD_SITL.h @@ -68,7 +68,7 @@ private: void load_font(); pthread_t thread; - AP_HAL::Semaphore *mutex; + HAL_Semaphore mutex; uint32_t counter; uint32_t last_counter; };