diff --git a/MAVProxy/modules/mavproxy_chat/__init__.py b/MAVProxy/modules/mavproxy_chat/__init__.py index c18849e07c..5fc90b6c62 100644 --- a/MAVProxy/modules/mavproxy_chat/__init__.py +++ b/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 -1 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,53 @@ 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 + 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 (-1 indicates we don't know result yet) + self.command_ack_waiting[mav_cmd] = -1 + + # 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, -1) + if result >= 0: + # 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) diff --git a/MAVProxy/modules/mavproxy_chat/chat_openai.py b/MAVProxy/modules/mavproxy_chat/chat_openai.py index d3b0f8c4ae..249d9eafb0 100644 --- a/MAVProxy/modules/mavproxy_chat/chat_openai.py +++ b/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): diff --git a/MAVProxy/modules/mavproxy_chat/chat_window.py b/MAVProxy/modules/mavproxy_chat/chat_window.py index ca2eda81d9..ad25c9116c 100644 --- a/MAVProxy/modules/mavproxy_chat/chat_window.py +++ b/MAVProxy/modules/mavproxy_chat/chat_window.py @@ -10,7 +10,7 @@ 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 @@ -18,7 +18,7 @@ def __init__(self, mpstate): 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()