Merge branch 'master' into xc_pydronecan

.github/workflows/python-package.yml

@@ -43,7 +43,7 @@ jobs:
 
     - name: Install pydronecan
       run: |
-        python setup.py build install
+        python -m pip install .
 
     - name: Run Tests from local
       run: |

dronecan/app/dynamic_node_id.py

@@ -29,8 +29,7 @@ class CentralizedServer(object):
 
     class AllocationTable(object):
         def __init__(self, path):
-            # Disabling same thread check on the assumption that the developer knows what they are doing.
-            self.db = sqlite3.connect(path, check_same_thread=False)  # @UndefinedVariable
+            self.db = sqlite3.connect(path)  # @UndefinedVariable
 
             self._modify('''CREATE TABLE IF NOT EXISTS `allocation` (
             `node_id`   INTEGER NOT NULL UNIQUE,
@@ -117,8 +116,16 @@ def get_allocation_table(self):
         return self._allocation_table.get_entries()
 
     def _handle_monitor_event(self, event):
+        if event.event_id not in (event.EVENT_ID_NEW, event.EVENT_ID_INFO_UPDATE):
+            return # don't care about nodes going offline or other such things
+        # unique ID might not be available if we see a node not participating in
+        # DNA and haven't got it or it didn't share that
         unique_id = event.entry.info.hardware_version.unique_id.to_bytes() if event.entry.info else None
-        self._allocation_table.set(unique_id, event.entry.node_id)
+        # set unique ID for this node ID (possibly to None in case we never get
+        # one) if we don't have one yet (though maybe we should raise a
+        # conflict if we do)
+        if self._allocation_table.get_unique_id(event.entry.node_id) is None:
+            self._allocation_table.set(unique_id, event.entry.node_id)
 
     def close(self):
         """Stops the instance and closes the allocation table storage.

dronecan/app/node_monitor.py

@@ -29,6 +29,7 @@ def __init__(self):
             self.info = None
             self.monotonic_timestamp = None
             self._remaining_retries = NodeMonitor.MAX_RETRIES
+            self._info_requested_at = 0
 
         @property
         def discovered(self):
@@ -166,7 +167,6 @@ def _on_node_status(self, e):
             new_entry = False
         except KeyError:
             entry = self.Entry()
-            entry._info_requested_at = 0
             self._registry[node_id] = entry
             new_entry = True
 
@@ -175,23 +175,21 @@ def _on_node_status(self, e):
         if new_entry:
             self._call_event_handlers(self.UpdateEvent(entry, self.UpdateEvent.EVENT_ID_NEW))
 
-        should_retry_now = entry.monotonic_timestamp - entry._info_requested_at > self.MIN_RETRY_INTERVAL
-
-        if not entry.discovered and should_retry_now and not e.node.is_anonymous:
-            entry._info_requested_at = entry.monotonic_timestamp
-            # noinspection PyProtectedMember
-            entry._register_retry()
-            e.node.request(uavcan.protocol.GetNodeInfo.Request(), node_id,  # @UndefinedVariable
-                           priority=self.TRANSFER_PRIORITY, callback=self._on_info_response)
+        if not entry.discovered and not e.node.is_anonymous:
+            should_retry_now = entry.monotonic_timestamp - entry._info_requested_at > self.MIN_RETRY_INTERVAL
+            if should_retry_now:
+                entry._info_requested_at = entry.monotonic_timestamp
+                # noinspection PyProtectedMember
+                entry._register_retry()
+                e.node.request(uavcan.protocol.GetNodeInfo.Request(), node_id,  # @UndefinedVariable
+                               priority=self.TRANSFER_PRIORITY, callback=self._on_info_response)
 
     def _on_info_response(self, e):
         if not e:
             return
 
         try:
             entry = self.get(e.transfer.source_node_id)
-            if entry._info_requested_at is None:
-                entry._info_requested_at = entry.monotonic_timestamp
         except KeyError:
             entry = self.Entry()
             self._registry[e.transfer.source_node_id] = entry

dronecan/driver/mavcan.py

@@ -34,7 +34,7 @@ def __init__(self, command, data):
         self.command = command
         self.data = data
 
-def io_process(url, bus, target_system, baudrate, tx_queue, rx_queue, exit_queue):
+def io_process(url, bus, target_system, baudrate, tx_queue, rx_queue, exit_queue, parent_pid):
     os.environ['MAVLINK20'] = '1'
 
     target_component = 0
@@ -44,8 +44,10 @@ def io_process(url, bus, target_system, baudrate, tx_queue, rx_queue, exit_queue
     signing_key = None
     last_loss_print_t = time.time()
     exit_proc = False
+    readonly = False
+
     def connect():
-        nonlocal conn, baudrate
+        nonlocal conn, baudrate, readonly
         conn = mavutil.mavlink_connection(url, baud=baudrate, source_system=250,
                                           source_component=mavutil.mavlink.MAV_COMP_ID_MAVCAN,
                                           dialect='ardupilotmega')
@@ -55,6 +57,8 @@ def connect():
         if signing_key is not None:
             conn.setup_signing(signing_key, sign_outgoing=True)
 
+        readonly = isinstance(conn, mavutil.mavlogfile)
+
     def reconnect():
         nonlocal exit_proc
         while True and not exit_proc:
@@ -71,6 +75,8 @@ def reconnect():
 
     def enable_can_forward():
         '''re-enable CAN forwarding. Called at 1Hz'''
+        if readonly:
+            return
         nonlocal last_enable, bus, target_system
         last_enable = time.time()
         conn.mav.command_long_send(
@@ -121,11 +127,17 @@ def handle_control_message(m):
         if (not exit_queue.empty() and exit_queue.get() == "QUIT") or exit_proc:
             conn.close()
             return
+        if os.getppid() != parent_pid:
+            # ensure we die when parent dies
+            conn.close()
+            return
         while not tx_queue.empty():
             if (not exit_queue.empty() and exit_queue.get() == "QUIT") or exit_proc:
                 conn.close()
                 return
             frame = tx_queue.get()
+            if readonly:
+                continue
             if isinstance(frame, ControlMessage):
                 handle_control_message(frame)
                 continue
@@ -200,7 +212,7 @@ def __init__(self, url, **kwargs):
 
         self.proc = multiprocessing.Process(target=io_process, name='mavcan_io_process',
                                             args=(url, self.bus, self.target_system, baudrate,
-                                            self.tx_queue, self.rx_queue, self.exit_queue))
+                                            self.tx_queue, self.rx_queue, self.exit_queue, os.getpid()))
         self.proc.daemon = True
         self.proc.start()
 

dronecan/version.py

@@ -9,5 +9,5 @@
 #         David Buzz
 #
 
-__version__ = '1.0.25'
+__version__ = '1.0.26'
 

examples/mavlink_tunnel.py

@@ -0,0 +1,104 @@
+#!/usr/bin/env python3
+import dronecan, time, threading, socket
+
+# get command line arguments
+from argparse import ArgumentParser
+parser = ArgumentParser(description='mavlink DroneCAN tunnel')
+parser.add_argument("--node-id", default=100, type=int, help="CAN node ID")
+parser.add_argument("--debug", action='store_true', help="enable debug")
+parser.add_argument("--tcp-host", type=str, default="localhost", help="tcp server host")
+parser.add_argument("--tcp-port", type=int, default=5790, help="tcp server port")
+parser.add_argument("--rate", type=float, default=10, help="broadcast rate Hz")
+parser.add_argument("uri", default=None, type=str, help="CAN URI")
+args = parser.parse_args()
+
+# Create a TCP/IP socket
+tcp_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+
+# Bind the socket to the address
+server_address = (args.tcp_host, args.tcp_port)
+tcp_sock.bind(server_address)
+
+# Listen for incoming connections
+tcp_sock.listen(1)
+connection = None
+
+def handle_Targetted(msg):
+    '''handle Targetted message'''
+    global last_mode, is_armed, connection
+    if args.debug:
+        # display the message on the console in human readable format
+        print(dronecan.to_yaml(msg))
+
+    if msg.message.target_node == args.node_id:
+        if msg.message.buffer:
+            if connection:
+                connection.sendall(bytes(msg.message.buffer))
+
+def publish_NodeInfo():
+    msg = dronecan.uavcan.protocol.NodeStatus()
+    msg.uptime_sec = int(time.time())
+    msg.health = msg.HEALTH_OK
+    msg.mode = msg.MODE_OPERATIONAL 
+    msg.submode = 0
+    msg.vendor_specific_status_code = 0
+    node.broadcast(msg)
+    if args.debug:
+        # display the message on the console in human readable format
+        print(dronecan.to_yaml(msg))
+
+def publish_MavlinkMsg(mav_pkt):
+    '''send Tunnel message'''
+    buffer = list(mav_pkt)
+    chunks = [buffer[i:i + 120] for i in range(0, len(buffer), 120)]
+    for chunk in chunks:
+        msg = dronecan.uavcan.tunnel.Targetted()
+        msg.serial_id = 1
+        msg.target_node = 10 
+        msg.protocol.protocol = 1 # MAVLink2
+        msg.baudrate = 115200
+        msg.options = msg.OPTION_LOCK_PORT
+        msg.buffer = chunk 
+        node.broadcast(msg)
+        if args.debug:
+            # display the message on the console in human readable format
+            print(dronecan.to_yaml(msg))
+
+def tcp_server_loop():
+    global connection
+    while True:
+        print(f'Waiting for GCS Connection on tcp:{args.tcp_host}:{args.tcp_port}...')
+        connection, client_address = tcp_sock.accept()
+        print("GCS connected")
+
+        try:
+            # Receive the data in small chunks and retransmit it
+            while True:
+                data = connection.recv(1024)
+                if data:
+                    publish_MavlinkMsg(data)
+
+        finally:
+            # Clean up the connection
+            connection.close()
+
+# Initializing a DroneCAN node instance.
+node = dronecan.make_node(args.uri, node_id=args.node_id, bitrate=1000000)
+
+# Initializing a node monitor, so we can see what nodes are online
+node_monitor = dronecan.app.node_monitor.NodeMonitor(node)
+
+node.periodic(1.0/args.rate, publish_NodeInfo)
+
+node.add_handler(dronecan.uavcan.tunnel.Targetted, handle_Targetted)
+
+tcp_server_loop_thread = threading.Thread(target=tcp_server_loop)
+tcp_server_loop_thread.start()
+
+# Running the node until the application is terminated or until first error.
+try:
+    node.spin()
+except KeyboardInterrupt:
+    tcp_server_loop.join()  
+    if connection:
+        connection.close()

examples/servo_control.py

@@ -0,0 +1,60 @@
+#!/usr/bin/env python3
+'''
+demonstrate servo monitoring and servo control
+the selected servo will be given sinisoidal control while printing position
+'''
+
+import dronecan
+import time
+import math
+from dronecan import uavcan
+
+# get command line arguments
+from argparse import ArgumentParser
+parser = ArgumentParser(description='test servo actuators')
+parser.add_argument("port", default=None, help="CAN URI")
+parser.add_argument("--bitrate", default=1000000, type=int, help="CAN bit rate")
+parser.add_argument("--node-id", default=125, type=int, help="CAN node ID")
+parser.add_argument("--target-node-id", default=100, type=int, help="target CAN node ID")
+parser.add_argument("--rate", default=20, type=float, help="servo update rate")
+parser.add_argument("--freq", default=1, type=float, help="servo sinisoid frequency")
+parser.add_argument("--actuator-min", default=-1, type=float, help="actuator min")
+parser.add_argument("--actuator-max", default=1, type=float, help="actuator max")
+parser.add_argument("--actuator-id", default=1, type=float, help="actuator ID")
+
+args = parser.parse_args()
+
+port = args.port
+
+# Initializing a DroneCAN node instance.
+node = dronecan.make_node(port, node_id=args.node_id, bitrate=args.bitrate)
+
+def handle_actuator_status(msg):
+    '''handle actuator status message'''
+    print(dronecan.to_yaml(msg))
+
+def control_actuator():
+    '''control for actuator, called at rate Hz'''
+    t = time.time()
+    sint = math.sin(t * args.freq * math.pi * 2)
+    amplitude = 0.5 * (args.actuator_max-args.actuator_min)
+    mid = args.actuator_min + amplitude
+    v = mid + sint * amplitude
+
+    req = uavcan.equipment.actuator.ArrayCommand()
+    cmd = uavcan.equipment.actuator.Command()
+    cmd.actuator_id = args.actuator_id
+    cmd.command_type = cmd.COMMAND_TYPE_UNITLESS
+    cmd.command_value = v
+    req.commands = [ cmd ]
+
+    node.broadcast(req)
+
+
+# Subscribe only to actuator status messages
+node.add_handler(uavcan.equipment.actuator.Status, handle_actuator_status)
+
+node.periodic(1.0/args.rate, control_actuator)
+
+while True:
+    node.spin()

examples/sim_battery.py

@@ -47,9 +47,6 @@ def publish_BatteryInfoAux():
 # Initializing a DroneCAN node instance.
 node = dronecan.make_node(args.uri, node_id=args.node_id, bitrate=1000000)
 
-# Initializing a node monitor, so we can see what nodes are online
-node_monitor = dronecan.app.node_monitor.NodeMonitor(node)
-
 # setup to publish battery info
 if args.balance:
     node.periodic(1.0/args.rate, publish_BatteryInfoAux)

examples/sim_servo.py

@@ -0,0 +1,50 @@
+#!/usr/bin/env python3
+'''
+simulate CAN actuators
+'''
+
+import dronecan, time, math
+
+# get command line arguments
+from argparse import ArgumentParser
+parser = ArgumentParser(description='simulate DroneCAN servo')
+parser.add_argument("--node-id", default=100, type=int, help="CAN node ID")
+parser.add_argument("--debug", action='store_true', help="enable debug")
+parser.add_argument("--rate", type=float, default=70, help="broadcast rate Hz")
+parser.add_argument("uri", default=None, type=str, help="CAN URI")
+args = parser.parse_args()
+
+def publish_ActuatorStatus():
+    '''send ActuatorStatus message'''
+    msg = dronecan.uavcan.equipment.actuator.Status()
+    msg.position = math.sin(time.time()*math.pi*2)
+    msg.force = 1
+    msg.speed = 2
+    msg.power_rating_pct = 17
+
+    # two actuators
+    msg.actuator_id = 3
+    node.broadcast(msg)
+
+    msg.actuator_id = 5
+    node.broadcast(msg)
+
+    if args.debug:
+        # display the message on the console in human readable format
+        print(dronecan.to_yaml(msg))
+
+# Initializing a DroneCAN node instance.
+node = dronecan.make_node(args.uri, node_id=args.node_id, bitrate=1000000)
+
+# setup to publish servo status
+node.periodic(1.0/args.rate, publish_ActuatorStatus)
+
+# Running the node until the application is terminated
+while True:
+    try:
+        node.spin()
+    except KeyboardInterrupt:
+        break
+    except dronecan.transport.TransferError as ex:
+        print(ex)
+        pass