diff --git a/cflib_groundstation/__pycache__/crazyflie_connection.cpython-38.pyc b/cflib_groundstation/__pycache__/crazyflie_connection.cpython-38.pyc index e6d5daf61a108acc7e4a3a17e2ac232b2a13e297..c4249331c166cdd16323d98a46de616822643865 100644 Binary files a/cflib_groundstation/__pycache__/crazyflie_connection.cpython-38.pyc and b/cflib_groundstation/__pycache__/crazyflie_connection.cpython-38.pyc differ diff --git a/cflib_groundstation/__pycache__/groundstation_socket.cpython-38.pyc b/cflib_groundstation/__pycache__/groundstation_socket.cpython-38.pyc index 8992ae1d5dc36d7cf4ca436430a295654b520bb4..8a732cdc4af311c7f155b3d8ce0fe128b8971509 100644 Binary files a/cflib_groundstation/__pycache__/groundstation_socket.cpython-38.pyc and b/cflib_groundstation/__pycache__/groundstation_socket.cpython-38.pyc differ diff --git a/cflib_groundstation/crazyflie_connection.py b/cflib_groundstation/crazyflie_connection.py index 440e1aa721581964dd420250621bff7ee3d69916..d744462a8e6e78a509ae1ac5e8d28e82e7eb0103 100644 --- a/cflib_groundstation/crazyflie_connection.py +++ b/cflib_groundstation/crazyflie_connection.py @@ -13,6 +13,8 @@ import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from queue import Queue +#import groundstation_socket as gs +from groundstation_socket import MessageTypeID from cflib.crazyflie.log import LogConfig @@ -78,13 +80,17 @@ class CrazyflieConnection: def BeginUpdate(): raise Exception def OverrideOuput(): - #Sends all setpoints for a given amount of time + """Sends all setpoints for a given amount of time""" + #TODO raise Exception def GetNodeIds(): raise Exception - def SetParam(self, group: str, name: str, value: float): + def SetParam(self, command): """ Set a crazyflie parameter value. """ + group = command['data'][0:1] + name = command['data'][2:3] + value = command['data'][4:7] try: if self.scf.is_link_open(): full_name = group + "." + name @@ -107,15 +113,36 @@ class CrazyflieConnection: print("Done setting param") self.param_callback_count += 1 - def GetParam(self, group: str, name: str): + def GetParam(self, command, outputQueue: Queue): #group: str, name: str, """ Retrieve parameter value from crazyflie toc. """ + #Bytes 0 and 1 are node ID, ie group + #Bytes 2 and 3 are node paramID, ie name + group = command['data'][0:1] + name = command['data'][2:3] + try: if self.scf.is_link_open(): - return self.scf.cf.param.values[group][name] + val = self.scf.cf.param.values[group][name] + except AttributeError: + val = -1.234567 pass - return -1.234567890 # 1234567890 should be pretty obvious that + + + data = bytearray(group) + data.append(name) + data.append(val) + + responsedata = { + "msg_type": (MessageTypeID.RESPPARAM_ID), + "msg_id": command['msg_id'], + "data_len": 8, + "data": data + } + outputQueue.put(responsedata) + + #return -1.234567890 # 1234567890 should be pretty obvious that # something has gone wrong. def SetSource(): raise Exception