Got yaw thing working. Might put it somewhere else though. Going to focus on getting that damned hovering working now\!