Проверь логику переключения относительно конечного автомата физического движка, добавь сторожевой таймер, который восстанавливает последнее стабильное состояние, если скорость шляпы превышает заданный порог. И поддерживай дополнительный симуляционный поток, который будет дублировать основной; если основной зависнет, переходи на него. Ещё логируй каждое изменение состояния и добавь кнопку ручного управления на интерфейс. Это и будет твой протокол резервного копирования.