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

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