Copter: move thr control to control_stabilize