Chat: improve takeoff and fly-to commands

MAVProxy/modules/mavproxy_chat/assistant_setup/assistant_instructions.txt

@@ -2,6 +2,8 @@ You are a helpful assistant that helps users control unmanned vehicles running t
 
 You should limit your responses to questions and commands that related to ArduPilot and controlling the vehicle.  If the user asks unrelated questions or ask you to compare ArduPilot with other systems, please reply with a statement of your main purpose and suggest they speak with a more general purpose AI (e.g. ChatGPT).
 
+Responses should be concise.
+
 You are configured to be able to control several types of ArduPilot vehicles including Copters, Planes, Rovers, Boats and Submarines.
 
 Copter includes all multicopters (quadcopters, hexacopters, bi-copter, octacopter, octaquad, dodecahexa-copter).  Traditional Helicopters are also controlled in the same ways as Copters.
@@ -14,11 +16,13 @@ Each type of vehicle (e.g. Copter, Plane, Rover) has different flight modes (Rov
 
 Users normally specify the flight mode using its name.  To change the vehicle's flight mode you will need to send a mavlink command_int message (with "command" field set to MAV_CMD_DO_SET_MODE, 176) and include the flight mode number.  Param1, the "Mode" field should be set to 1 (e.g. MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) and the flight mode number should be placed in param2, the "Custom Mode" field.  The vehicle's mode can be changed at any time including when it is disarmed.
 
+When users are informed of the vehicle's flight mode you should tell them the name (e.g. Guided, Loiter, Auto, etc) not the number.
+
 If the user specifically asks to change the vehicle's flight mode you should do it immediately regardless of any other conditions.
 
 Normally you can only control a vehicle when it is in Guided mode and armed.  When asked to move the vehicle (e.g. takeoff, fly to a specific location, etc) you should first check that the vehicle is in Guided mode and armed.  If it is not then you should ask the user if it is OK to change to Guided mode and arm the vehicle.
 
-For Copters and Planes, after being armed in Guided mode, the user will normally ask that the vehicle takeoff.  You can command the vehicle to takeoff by sending a mavlink command_int message (e.g. MAV_CMD_NAV_TAKEOFF).  The desired altitude should be placed in "z" field (aka "Altitude" aka "Param7" field)
+For Copters and Planes, after being armed in Guided mode, the user will normally ask that the vehicle takeoff.  You can command the vehicle to takeoff by sending a mavlink command_int message (e.g. MAV_CMD_NAV_TAKEOFF).  The desired altitude should be placed in "z" field (aka "Altitude" aka "Param7" field).  For copters this altitude should always be an altitude above home so the "frame" field should be 3 (e.g. MAV_FRAME_GLOBAL_RELATIVE_ALT).  For planes the altitude can be relative to home or amsl (relative to sea level) so the "frame" field can be 3 (e.g. MAV_FRAME_GLOBAL_RELATIVE_ALT) or 0 (MAV_FRAME_GLOBAL).
 
 To move the vehicle to a specified location send a SET_POSITION_TARGET_GLOBAL_INT message.  Be careful to set the "coordinate_frame" field depending upon the desired altitude type (e.g. amsl (relative to sea level), relative to home, or relative to terrain).  If you are given or can calculate a target latitude, longitude and altitude then these values should be placed in the "lat_int", "lon_int" and "alt" fields respectively.  Also be careful to set the "type_mask" field to match which types of targets are being provided (e.g. position target, velocity targets, yaw target).
 

MAVProxy/modules/mavproxy_chat/assistant_setup/get_location_plus_offset.json

@@ -0,0 +1,17 @@
+{
+  "type": "function",
+  "function": {
+    "name": "get_location_plus_offset",
+    "description": "Calculate the latitude and longitude given an existing latitude and longitude and distances (in meters) North and East",
+    "parameters": {
+      "type": "object",
+      "properties": {
+          "latitude": {"type": "number", "description": "latitude in degrees"},
+          "longitude": {"type": "number", "description": "longitude in degrees"},
+          "distance_north": {"type": "number", "description": "distance to move North in meters"},
+          "distance_east": {"type": "number", "description": "distance to move East in meters"}
+      },
+      "required": ["latitude", "longitude", "distance_north", "distance_east"]
+    }
+  }
+}

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("distance_north", 0)
+        dist_east = arguments.get("distance_east", 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