Initial code from GPS.ino (multiwii svn)
Частично запилен код стабилизации по двум сонарам - путём переделки кода GPS.