Control doesnt run before motor startup
Fixed mot auto bug, changed pi constants, torque.z hasn't got integral control

yaw can't be properly measured, integral control has no sense.