А по поводу отладки я даже и не знаю, что посоветовать.
“надзиратель” на WDT сможешь “осилить” ? Без reset, только вектор и обратно в код?
Или назначить несколько глобальных переменных типа int8 = состояние функции ( 0-не работаю; 1-работаю; и тд ) и int32 в качестве счетчика вызовов. на каждую функцию.
Замутить шедулер по типу:
Func_1
Func_2
и так далее с нужным количеством вызовов в секунду. (или придумай свой квант, допустим 0.25 сек)
Здеся вариантов просто вагон.
Сделай небольшой лог на экране и смотри за всем. Это сложно по началу, но на WDT можно сделать отладчик в отладчике очень даже запросто.
Ну как-то так.