...Он использует данные от аппаратуры радиоуправления (PPM), нескольких датчиков (гироскоп, акселерометр, барометр, гпс, магнитометр по шине i2c) и вычисляет скорость для каждого отдельного мотора/пропеллера.
На Вашем какие датчики задействованы? Чем нибудь плавное "уплывание" по высоте и по горизонту контролируете?