Fixed a few logical bugs Navigation.c/GPS.c