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