PatrikE's Fixed Wing RTH

There's a wrap function in gps.c, what was wrong with just using that?