Chat: reliability enhancements

MAVProxy/modules/mavproxy_chat/__init__.py

@@ -12,7 +12,9 @@
 from MAVProxy.modules.lib import mp_module
 from MAVProxy.modules.lib import mp_util
 from MAVProxy.modules.mavproxy_chat import chat_window
+from pymavlink import mavutil
 from threading import Thread
+import time
 
 class chat(mp_module.MPModule):
     def __init__(self, mpstate):
@@ -26,6 +28,11 @@ def __init__(self, mpstate):
         # keep reference to mpstate
         self.mpstate = mpstate
 
+        # a dictionary of command_ack mavcmds we are waiting for
+        # key is the mavcmd, value is None or the MAV_RESULT (e.g. 0 to 9)
+        # we assume we will never be waiting for two of the same mavcmds at the same time
+        self.command_ack_waiting = {}
+
         # run chat window in a separate thread
         self.thread = Thread(target=self.create_chat_window)
         self.thread.start()
@@ -34,7 +41,7 @@ def __init__(self, mpstate):
     def create_chat_window(self):
         if mp_util.has_wxpython:
             # create chat window
-            self.chat_window = chat_window.chat_window(self.mpstate)
+            self.chat_window = chat_window.chat_window(self.mpstate, self.wait_for_command_ack)
         else:
             print("chat: wx support required")
 
@@ -55,6 +62,54 @@ def cmd_chat(self, args):
     def show(self):
         self.chat_window.show()
 
+    # handle mavlink packet
+    def mavlink_packet(self, m):
+        if m.get_type() == 'COMMAND_ACK':
+            self.handle_command_ack(m)
+
+    # handle_command_ack.  should be called if module receives a COMMAND_ACK command
+    def handle_command_ack(self, m):
+        # return immediately if we are not waiting for this command ack
+        if m.command not in self.command_ack_waiting:
+            return
+
+        # throw away value if result in progress
+        if m.result == mavutil.mavlink.MAV_RESULT_IN_PROGRESS:
+            return
+
+        #  set the mav result for this command
+        self.command_ack_waiting[m.command] = m.result
+
+    # wait for COMMAND_ACK with the specified mav_cmd
+    # this should be called immediately after sending a command_long or command_int
+    # mav_cmd should be set to the command id that was sent
+    # returns MAV_RESULT if command ack received, False if timed out
+    # Note: this should not be called from the main thread because it blocks for up to "timeout" seconds
+    def wait_for_command_ack(self, mav_cmd, timeout=1):
+        # error if we are already waiting for this command ack
+        if mav_cmd in self.command_ack_waiting:
+            print("chat: already waiting for command ack for mavcmd:" + str(mav_cmd))
+            return False
+
+        # add to list of commands we are waiting for (None indicates we don't know result yet)
+        self.command_ack_waiting[mav_cmd] = None
+
+        # wait for ack, checking for a response every 0.1 seconds
+        start_time = time.time()
+        while time.time() - start_time < timeout:
+            # check if we got the ack we were waiting for
+            result = self.command_ack_waiting.get(mav_cmd, None)
+            if result is not None:
+                # remove from list of commands we are waiting for
+                del self.command_ack_waiting[mav_cmd]
+                return result
+            time.sleep(0.1)
+
+        # timeout, remove from list of commands we are waiting for
+        # return False indicating timeout
+        del self.command_ack_waiting[mav_cmd]
+        return False
+
 # initialise module
 def init(mpstate):
     return chat(mpstate)

MAVProxy/modules/mavproxy_chat/assistant_setup/assistant_instructions.txt

@@ -12,6 +12,8 @@ Planes includes all fixed wing vehicles including normal planes and quadplanes.
 
 Rovers (aka cars) and Boats are controlled in the same way.
 
+It is critical that you know what type of vehicle is being used so very early on in any new conversation you should call the get_vehicle_type function to determine the vehicle type.
+
 Each type of vehicle (e.g. Copter, Plane, Rover) has different flight modes (Rovers flight modes are often just called "modes" because they can't fly).  Each flight mode has a number and name.  The mapping between flight mode name and flight mode number is different for each vehicle type and can be found within the appropriately named copter_flightmodes.txt, plane_flightmodes.txt, rover_modes.txt and sub_modes.txt files.
 
 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.
@@ -20,14 +22,20 @@ When users are informed of the vehicle's flight mode you should tell them the na
 
 If the user specifically asks to change the vehicle's flight mode you should do it immediately regardless of any other conditions.
 
+Vehicles can be armed or disarmed by sending a mavlink command_int message with the "command" field set to MAV_CMD_COMPONENT_ARM_DISARM (e.g 400) and the "param1" field (aka "Arm") set to 1 to arm or 0 to disarm.  Arming the vehicle sometimes fails because of pre-arm checks.  Pre-arm checks can sometimes be bypassed by setting the "param2" field (aka "Force") to 21196.  You should only try to force arming if specifically asked by the user.  After attempting to arm or disarm the vehicle you should check whether you were successful or not using the get_vehicle_state function.
+
 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 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).
+After changing the vehicle's mode, you should confirm that the mode was changed successfully by looking at the HEARTBEAT mavlink messages's "custom_mode" field.
+
+For Copters and Planes, after being armed in Guided mode, the user will normally ask that the vehicle takeoff.  Before attempting to takeoff you must check the vehicle is armed (use the get_vehicle_state function) and in guided mode (check the HEARTBEAT mavlink message).  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).
 
 If a user is not clear about the altitude type then it is normally safe to assume they mean altitude above home.  Sometimes "altitude above home" is referred to as "relative altitude".
 
+If the user asks to change the yaw (aka heading) of the vehicle, you should send a command_int message with the "command" field set to MAV_CMD_CONDITION_YAW (e.g. 115).  Normally the desired yaw angle should be placed in param1 (aka "Angle") and param2 ("Angular Speed"), param3 ("Direction") and param4 ("Relative") can be left as zero but if the user specifies the angle is relative to the vehicle's current heading then param3 ("Direction") should be set to -1 to rotate left (e.g. counter-clockwise) or +1 to rotate right (e.g. clockwise) and param4 (e.g. "Relative") should be set to 1.
+
 The short form of "altitude" is "alt".
 The short form of "latitude" is "lat".
 The short form of "longitude" is "lat".

MAVProxy/modules/mavproxy_chat/chat_openai.py

@@ -20,13 +20,16 @@
     exit()
 
 class chat_openai():
-    def __init__(self, mpstate, status_cb=None):
+    def __init__(self, mpstate, status_cb=None, wait_for_command_ack_fn=None):
         # keep reference to mpstate
         self.mpstate = mpstate
 
         # keep reference to status callback
         self.status_cb = status_cb
 
+        # keep reference to wait_for_command_ack_fn
+        self.wait_for_command_ack_fn = wait_for_command_ack_fn
+
         # initialise OpenAI connection
         self.client = None
         self.assistant = None
@@ -393,7 +396,28 @@ def send_mavlink_command_int(self, arguments):
         y = arguments.get("y", 0)
         z = arguments.get("z", 0)
         self.mpstate.master().mav.command_int_send(target_system, target_component, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z)
-        return "command_int sent"
+
+        # wait for command ack
+        mav_result = self.wait_for_command_ack_fn(command)
+
+        # check for timeout
+        if mav_result is None:
+            print("send_mavlink_command_int: timed out")
+            return "command_int timed out"
+
+        # update assistant with result
+        if mav_result == mavutil.mavlink.MAV_RESULT_ACCEPTED:
+            return "command_int succeeded"
+        if mav_result == mavutil.mavlink.MAV_RESULT_FAILED:
+            return "command_int failed"
+        if mav_result == mavutil.mavlink.MAV_RESULT_DENIED:
+            return "command_int denied"
+        if mav_result == mavutil.mavlink.MAV_RESULT_UNSUPPORTED:
+            return "command_int unsupported"
+        if mav_result == mavutil.mavlink.MAV_RESULT_TEMPORARILY_REJECTED:
+            return "command_int temporarily rejected"
+        print("send_mavlink_command_int: received unexpected command ack result")
+        return "command_int unknown result"
 
     # send a mavlink send_mavlink_set_position_target_global_int message to the vehicle
     def send_mavlink_set_position_target_global_int(self, arguments):

MAVProxy/modules/mavproxy_chat/chat_window.py

@@ -10,15 +10,15 @@
 from threading import Thread, Lock
 
 class chat_window():
-    def __init__(self, mpstate):
+    def __init__(self, mpstate, wait_for_command_ack_fn):
         # keep reference to mpstate
         self.mpstate = mpstate
 
         # lock to prevent multiple threads sending text to the assistant at the same time
         self.send_lock = Lock()
 
         # create chat_openai object
-        self.chat_openai = chat_openai.chat_openai(self.mpstate, self.set_status_text)
+        self.chat_openai = chat_openai.chat_openai(self.mpstate, self.set_status_text, wait_for_command_ack_fn)
 
         # create chat_voice_to_text object
         self.chat_voice_to_text = chat_voice_to_text.chat_voice_to_text()