Merge pull request #16 from ahcorde/master

LOITER + RTL
Merge pull request #17 from ahcorde/master

GPS issues
Merge pull request #18 from ahcorde/master

Loiter + GPS bad results
Merge pull request #19 from ahcorde/master

Change mode - STABILIZE, ALT_HOLD, LOITER and RTL
Merge pull request #20 from ahcorde/master

Change mode - STABILIZE, ALT_HOLD, LOITER and RTL