diff --git a/MAVProxy/modules/mavproxy_chat/chat_openai.py b/MAVProxy/modules/mavproxy_chat/chat_openai.py index d57d1c584c..66944eae92 100644 --- a/MAVProxy/modules/mavproxy_chat/chat_openai.py +++ b/MAVProxy/modules/mavproxy_chat/chat_openai.py @@ -11,6 +11,7 @@ import time from datetime import datetime import json +import math try: from openai import OpenAI @@ -183,6 +184,20 @@ def handle_function_call(self, run): recognised_function = True output = json.dumps(self.get_vehicle_location_and_yaw()) + # get_location_plus_offset + if tool_call.function.name == "get_location_plus_offset": + recognised_function = True + try: + arguments = json.loads(tool_call.function.arguments) + except: + print("chat::handle_function_call: get_location_plus_offset: failed to parse arguments") + output = "get_location_plus_offset: failed to parse arguments" + try: + output = json.dumps(self.get_location_plus_offset(arguments)) + except: + print("chat::handle_function_call: get_location_plus_offset: failed to calc location") + output = "get_location_plus_offset: failed to get location" + # send mavlink command_int if tool_call.function.name == "send_mavlink_command_int": recognised_function = True @@ -294,6 +309,20 @@ def get_vehicle_location_and_yaw(self): } return location + # Calculate the latitude and longitude given distances (in meters) North and East + def get_location_plus_offset(self, arguments): + lat = arguments.get("latitude", 0) + lon = arguments.get("longitude", 0) + dist_north = arguments.get("dNorth", 0) + dist_east = arguments.get("dEast", 0) + lat_lon_to_meters_scaling = 89.8320495336892 * 1e-7 + lat_diff = dist_north * lat_lon_to_meters_scaling + lon_diff = dist_east * lat_lon_to_meters_scaling / max(0.01, math.cos(math.radians((lat+lat_diff)/2))) + return { + "latitude": self.wrap_latitude(lat + lat_diff), + "longitude": self.wrap_longitude(lon + lon_diff) + } + # send a mavlink command_int message to the vehicle def send_mavlink_command_int(self, arguments): target_system = arguments.get("target_system", 1) @@ -336,3 +365,19 @@ def send_mavlink_set_position_target_global_int(self, arguments): yaw_rate = arguments.get("yaw_rate", 0) self.mpstate.master().mav.set_position_target_global_int_send(time_boot_ms, target_system, target_component, coordinate_frame, type_mask, lat_int, lon_int, alt, vx, vy, vz, afx, afy, afz, yaw, yaw_rate) return "set_position_target_global_int sent" + + # wrap latitude to range -90 to 90 + def wrap_latitude(self, latitude_deg): + if latitude_deg > 90: + return 180 - latitude_deg + if latitude_deg < -90: + return -(180 + latitude_deg) + return latitude_deg + + # wrap longitude to range -180 to 180 + def wrap_longitude(self, longitude_deg): + if longitude_deg > 180: + return longitude_deg - 360 + if longitude_deg < -180: + return longitude_deg + 360 + return longitude_deg