Средства и методы разработки

Я в таких случаях рефакторил последовательно. Запускал РТОС и в ней единственной задачей бывшую main, прерывания оставлял в прежнем виде. Далее тестирование и смотрю, что можно откусить от суперлупа и/или прерывания в отдельную задачу. Опять тестирование и смотрим дальше. В конце в бывшей main остаётся некая лапша, которая хрен пойми как работает, но всё ещё свои функции выполняет. А объём кода сокращается до уровня, когда можно переписать полностью не особо напрягаясь, и можно сморщив нос выкинуть на помойку.