и как это отлаживать? И самое главное - кого, прошивку или qgc? :) Воспроизводимый тесткейс это способ ГАРАНТИРОВАННО получить ошибку при пошаговом проходе.
Ардуинский Serial, на коий этот весьма похож, ранее имел баг, иногда дублируя символы когда прерывание невовремя случалось, но таки ж там 8-битный проц а тут 32, и обращение к указателям атомарны. Могу сделать версию где вся работа с очередью будет делаться при запрещенных прерываниях. Если пройдет то дело в этом - но у меня нет проблем с загрузкой параметров в МП по радиомодему, так что в качестве workaround-а могу предложить использовать МП