What you propose may work but it is not, in my opinion, the best solution.

I would really suggest following the idea I presented before.

I can attest to the fact that there is always a strong enticement to try to "fix up" what we have already made. However often those fixings are not much more than a band aid that covers up a deeper wound. In many cases it can be best to tear up the old papers, delete the files and start with a clean slate.

I routinely write MCU applications with all the physical I/O handled completely in interrupt and timer service routines. It leads to modular solutions that are easy to debug, easy to port to other projects, and limit the exposure to the type of concurrency problem that your program appears to have.

Michael Karas