def Global_direction():
global path_now, heading, latitude, longitude, distance_to_current_goal
path_now_gps_y, path_now_gps_x = GPS.BD_to_GPS(path_now[1], path_now[0])
#print("path now in baidu",path_now)
#print("path now in gps",path_now_gps_x, path_now_gps_y)
#print("now gps",longitude,latitude)
x1, y1 = millerToXY(longitude, latitude)
x2, y2 = millerToXY(path_now_gps_x, path_now_gps_y)
print('current goal', x2 - x1, y2 - y1)
distance_to_current_goal = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
direction_x = 90.0 - math.atan2((y2 - y1),
(x2 - x1)) * 180 / math.pi - heading
while (direction_x < -360):
direction_x += 360
while (direction_x > 360):
direction_x -= 360
if (direction_x < -180):
direction_x += 360
if (direction_x > 180):
direction_x -= 360
#print('global_direction',direction_x)
return direction_x
