diff --git a/dronecan/driver/common.py b/dronecan/driver/common.py index 171f019..5622c6b 100644 --- a/dronecan/driver/common.py +++ b/dronecan/driver/common.py @@ -53,6 +53,7 @@ def __str__(self): return "%12.6f %12.6f %s %s '%s'" % \ (self.ts_monotonic, self.ts_real, id_str, hex_data, ascii_data) + @staticmethod def dlc_to_datalength(dlc): # Data Length Code 9 10 11 12 13 14 15 # Number of data bytes 12 16 20 24 32 48 64 @@ -72,6 +73,7 @@ def dlc_to_datalength(dlc): return 48 return 64 + @staticmethod def datalength_to_dlc(data_length): if (data_length <= 8): return data_length @@ -134,10 +136,10 @@ def set_filter_list(self, ids): '''set list of message IDs to accept, sent to the remote capture node with mavcan''' pass - def get_filter_list(self, ids): - '''get list of message IDs to accept, None means accept all''' + def get_filter_list(self): + '''get the current filter list''' return None - + def set_bus(self, busnum): '''set the remote bus number to attach to''' pass @@ -146,18 +148,14 @@ def get_bus(self): '''get the remote bus number we are attached to''' return None - def get_filter_list(self): - '''get the current filter list''' - return None - - def set_bus(self, busnum): - '''set the remote bus number to attach to''' - pass - def set_signing_passphrase(self, passphrase): '''set MAVLink2 signing passphrase''' pass + def set_parameter(self, name, value): + '''set parameter on the remote device''' + pass + def stream_progress(self): '''stream progress of the current stream''' pass diff --git a/dronecan/driver/mavcan.py b/dronecan/driver/mavcan.py index 4f0b9bf..f6086f6 100644 --- a/dronecan/driver/mavcan.py +++ b/dronecan/driver/mavcan.py @@ -119,6 +119,8 @@ def handle_control_message(m): nonlocal signing_key signing_key = m.data conn.setup_signing(signing_key, sign_outgoing=True) + elif m.command == "SetParam": + conn.param_set_send(m.data[0], m.data[1]) connect() enable_can_forward() @@ -248,6 +250,7 @@ def send_frame(self, frame): self._tx_hook(frame) self.tx_queue.put_nowait(frame) + @staticmethod def is_mavlink_port(device_name, baudrate): '''check if a device is sending mavlink''' os.environ['MAVLINK20'] = '1' @@ -263,8 +266,8 @@ def set_filter_list(self, ids): self.filter_list = ids self.tx_queue.put_nowait(ControlMessage('FilterList', self.filter_list)) - def get_filter_list(self, ids): - '''set list of message IDs to accept, sent to the remote capture node with mavcan''' + def get_filter_list(self): + '''get list of message IDs to accept''' return self.filter_list def set_bus(self, busnum): @@ -278,9 +281,12 @@ def get_bus(self): '''get the remote bus number we are attached to''' return self.bus+1 - def get_filter_list(self): - '''get the current filter list''' - return self.filter_list + def set_parameter(self, name, value): + '''set parameter in the remote air vehicle''' + if len(name) > 16: + raise DriverError('Parameter name %s too long' % name) + + self.tx_queue.put_nowait(ControlMessage('SetParam', (name, value))) def passphrase_to_key(self, passphrase): '''convert a passphrase to a 32 byte key'''