diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp index c4707f6624..6f498534d5 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.cpp +++ b/libraries/AP_IOMCU/AP_IOMCU.cpp @@ -157,6 +157,7 @@ void AP_IOMCU::event_failed(uint8_t event) void AP_IOMCU::thread_main(void) { thread_ctx = chThdGetSelfX(); + chEvtSignal(thread_ctx, initial_event_mask); uart.begin(1500*1000, 256, 256); uart.set_blocking_writes(false); @@ -543,6 +544,9 @@ void AP_IOMCU::trigger_event(uint8_t event) { if (thread_ctx != nullptr) { chEvtSignal(thread_ctx, EVENT_MASK(event)); + } else { + // thread isn't started yet, trigger this event once it is started + initial_event_mask |= EVENT_MASK(event); } } diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h index 7aca6ead52..911bf0cad8 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.h +++ b/libraries/AP_IOMCU/AP_IOMCU.h @@ -112,6 +112,8 @@ private: // IOMCU thread thread_t *thread_ctx; + eventmask_t initial_event_mask; + // time when we last read various pages uint32_t last_status_read_ms; uint32_t last_rc_read_ms;