chat: handle command ack

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/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()