Add wait times to take off and RTL. Add fix yaw thread and compass
testing app (they will provably be deprecated in the future)