finish yaw control, tune altitude correction