compass = compass_device(COMPASS_PORT)
### Variable Init
latitude = 0.0
longitude = 0.0
heading = 0.0
time = ""
### MAIN LOOP
while True:
try: # Handler to Catch Ctrl-C
### Get Latest Data
gps_device.update()
time = gps_device.get_datetime()
latitude = gps_device.get_current_latitude()
longitude = gps_device.get_current_longitude()
heading = compass.get_heading_compensated(COMPASS_OFFSET)
if latitude == 0.0 or longitude == 0.0: # Skip over empty GPS Readings
continue
### Calculate Distance and Bearing to Target
curr_location_point = point.Point(latitude, longitude)
meters_to_waypoint = curr_waypoint_point.distance(curr_location_point) * 1000 # Convert kM result to Meters
bearing_to_waypoint = curr_location_point.bearing(curr_waypoint_point)
### Determine Speed
if meters_to_waypoint <= 4.2: # Made it!
print("*******WP REACHED!*******"),
