mavcmd: TOL cmds: TOL from current GPS position - initial commit

Needs verification, since it doesn't always enter the gps callback functions. May be sync issue.