-First try of exiting PID cleanly and comming to initial state
-First try of exiting PID cleanly and comming to initial state

vjaunet/QUADCOPTER,Anti-code/QUADCOPTER,karthiknrao/QUADCOPTER,vjaunet/QUADCOPTER,Anti-code/QUADCOPTER,karthiknrao/QUADCOPTER,vjaunet/QUADCOPTER,Anti-code/QUADCOPTER,karthiknrao/QUADCOPTER,karthiknrao/QUADCOPTER,Anti-code/QUADCOPTER,vjaunet/QUADCOPTER