import math

import time
from collections import namedtuple
import gps
coordinates = namedtuple("coordinates", ["latitude", "longitude"])
session = gps.gps(mode=gps.WATCH_ENABLE)
def get_bearing(x1, y1, x2, y2):
bearing = math.atan2(y2 - y1, x2 - x1) * (180 / math.pi)
return (bearing + 360) % 360
def get_distance(lat1, lon1, lat2, lon2):
return 2 * math.asin(math.sqrt(math.pow(sin((lat2-lat1)/2), 2)
+ math.cos(lat1) * math.cos(lat2) * math.pow(sin((lon2-lon1)/2),
2))) * 6371 * 1000
def update_waypoint(current_position, waypoints,
current_waypoint_index):
if current_waypoint_index < len(waypoints) - 1:
current_waypoint_index += 1
current_waypoint = waypoints[current_waypoint_index]
print("Navigating to waypoint", current_waypoint_index +
1)
return current_waypoint, current_waypoint_index
else:
print("Reached the final destination")
return None, None
try:
waypoints = [
coordinates(
float(input("Enter waypoint 1 latitude: ")),
float(input("Enter waypoint 1 longitude: ")),
),
coordinates(
float(input("Enter waypoint 2 latitude: ")),
float(input("Enter waypoint 2 longitude: ")),
),
coordinates(
float(input("Enter waypoint 3 latitude: ")),
float(input("Enter waypoint 3 longitude: ")),
),
]
flag = 0
current_waypoint_index = 0
current_waypoint = waypoints[current_waypoint_index]
while True:
if not (session.valid):
print('TPV')
time.sleep(2)
continue
if flag == 0:
if gps.isfinite(session.fix.latitude) and
gps.isfinite(session.fix.longitude):
current_position =
coordinates(session.fix.latitude, session.fix.longitude)
initial_distance =
get_distance(current_position.latitude,
current_position.longitude,
waypoints[current_waypoint_index].latitude,
waypoints[current_waypoint_index].longitude)
initial_bearing =
get_bearing(current_position.longitude, current_position.latitude,
waypoints[current_waypoint_index].longitude,
waypoints[current_waypoint_index].latitude)
print("Initial Distance:", initial_distance)
print("Initial Bearing:", initial_bearing)
flag = 1
if gps.isfinite(session.fix.latitude) and
gps.isfinite(session.fix.longitude):
current_position = coordinates(session.fix.latitude,
session.fix.longitude)
current_distance =
get_distance(current_position.latitude,
current_position.longitude,
waypoints[current_waypoint_index].latitude,
waypoints[current_waypoint_index].longitude)
current_bearing =
get_bearing(current_position.longitude, current_position.latitude,
waypoints[current_waypoint_index].longitude,
waypoints[current_waypoint_index].latitude)
print("Current Distance:", current_distance)
print("Current Bearing:", current_bearing)
if current_distance <= 0:
current_waypoint, current_waypoint_index =
update_waypoint(current_position, waypoints, current_waypoint_