Minor changes

disable pitch_decouple when disarmed;
remove unnecessary declaration of have_vertical_velocity();
Fix some warning and clean up code
pitch_decouple_factor initialise fix
adjust acceleration to target roll/pitch angle

based on vertical acceleration
fix earth frame to body frame conversion
Add GPS glitch condition in GPS yaw control mode
Minor changes and comments