Конечно. Просто добавь простую проверку высоты в главный цикл – если высотомер показывает, что ты выше 90 процентов от максимальной высоты, убавь тягу до нуля или сократи её вдвое. Добавь небольшую задержку, чтобы не дергалось, и тогда это дрожание не столкнётся с потолком.

Можно просто увеличить частоту ШИМ до 20 килогерц, и уменьшать подачу газа за один цикл, когда срабатывает флаг датчика, а потом держать её до тех пор, пока флаг не сбросится. Так снижение будет почти мгновенное, без этой задержки в 0.2 секунды, и дрон всё равно будет знать, когда подниматься. Главное – чтобы прерывание не перегрузило систему, коротко и ясно – и тогда не получишь этого размытия перед тем, как всё рухнет.

Поставь небольшой таймаут сторожевого таймера, ну, типа 50 миллисекунд, и сбрасывай его внутри прерывания после падения оборотов. Если прерывание зависнет, сторожевой таймер сработает и дроном приземлится безопасно. Следи, чтобы прерывание выполнялось меньше пары микросекунд, и твоя камера не будет смазываться.