Апдейт виртуального мультикоптера

Перепиливаю систему стабилизации мультикоптера. Переписал код работающий с ориентацией коптера с углов Эйлера на кватернионы, всё стало клёвее, ошибка считается в локальном пространстве коптера, представляет собой вектор “наклона” коптера относительно заданной ориентации. Dot-ом для каждого считаю развешивание ошибки между движками (на рисунке – жёлтые линии выходящие из движков), получается прям очень удобная система! Она не зависит от количества и расположения движков, всё автоматом развешивается и работает.  Осталось накинуть обратно PID контроллеры и квадрик снова будет летать )