Просто дабавил к выходу ПИД пропорциональную корректировку с изменяемым коэффициентом. Полет нормальный.