Feature/spider refactor can

config/spider.json

@@ -0,0 +1,40 @@
+{
+  "version": 2,
+  "robot": {
+    "modules": {
+      "app": {
+          "driver": "application",
+          "in": [],
+          "out": ["desired_speed"],
+          "init": {
+            "max_speed": 0.5
+          }
+      },
+      "spider": {
+          "driver": "spider",
+          "in": ["can", "move"],
+          "out": ["can"],
+          "init": {}
+      },
+      "can": {
+          "driver": "can",
+          "in": ["raw", "can"],
+          "out": ["can", "raw"],
+          "init": {}
+      },
+      "serial": {
+          "driver": "serial",
+          "in": ["raw"],
+          "out": ["raw"],
+          "init": {"port": "/dev/ttyS0", "speed": 115200,
+                   "rtscts":true, "reset":true}
+      }
+    },
+    "links": [["app.desired_speed", "spider.move"],
+              ["spider.status", "app.status"],
+              ["spider.can", "can.can"],
+              ["can.can", "spider.can"],
+              ["serial.raw", "can.raw"], 
+              ["can.raw", "serial.raw"]]
+  }
+}

config/test-spider.json

@@ -4,19 +4,27 @@
     "modules": {
       "spider": {
           "driver": "spider",
-          "in": ["raw"],
+          "in": ["can"],
           "out": ["can"],
           "init": {}
       },
-      "spider_serial": {
+      "can": {
+          "driver": "can",
+          "in": ["raw", "can"],
+          "out": ["can", "raw"],
+          "init": {}
+      },
+      "serial": {
           "driver": "serial",
           "in": ["raw"],
           "out": ["raw"],
           "init": {"port": "/dev/ttyS0", "speed": 115200,
                    "rtscts":true, "reset":true}
       }
     },
-    "links": [["spider_serial.raw", "spider.raw"], 
-              ["spider.can", "spider_serial.raw"]]
+    "links": [["spider.can", "can.can"],
+              ["can.can", "spider.can"],
+              ["serial.raw", "can.raw"], 
+              ["can.raw", "serial.raw"]]
   }
 }

osgar/drivers/spider.py

@@ -9,15 +9,8 @@
 from osgar.bus import BusShutdownException
 
 
-CAN_BRIDGE_READY = b'\xfe\x10'  # CAN bridge is ready to accept configuration commands
-CAN_BRIDGE_SYNC = b'\xFF'*10    # CAN bridge synchronization bytes
-CAN_SPEED_1MB = b'\xfe\x57'     # configure CAN bridge to communicate on 1Mb CAN network
-CAN_BRIDGE_START = b'\xfe\x31'  # start bridge
-
-
-def CAN_packet(msg_id, data):
-    header = [(msg_id>>3) & 0xff, (msg_id<<5) & 0xe0 | (len(data) & 0xf)]
-    return bytes(header + data)
+def CAN_triplet(msg_id, data):
+    return [msg_id, bytes(data), 0]  # flags=0, i.e basic addressing
 
 
 class Spider(Thread):
@@ -27,9 +20,6 @@ def __init__(self, config, bus):
         self.setDaemon(True)
 
         self.bus = bus
-        self.buf = b''
-
-        self.can_bridge_initialized = False
         self.status_word = None  # not defined yet
         self.wheel_angles = None  # four wheel angles as received via CAN
         self.zero_steering = None  # zero position of all 4 wheels
@@ -38,22 +28,7 @@ def __init__(self, config, bus):
         self.alive = 0  # toggle with 128
         self.desired_angle = None  # in Spider mode desired weels direction
         self.desired_speed = None
-
-    @staticmethod
-    def split_buffer(data):
-        # skip 0xFF prefix bytes (CAN bridge control bytes)
-        data = data.lstrip(b'\xff')
-
-        if len(data) >= 2:
-            # see https://en.wikipedia.org/wiki/CAN_bus
-            header = data[:2]
-            rtr = (header[1] >> 4) & 0x1  # Remote transmission request
-            size = (header[1]) & 0x0f
-            if rtr:
-                return data[2:], header
-            elif len(data) >= 2 + size:
-                return data[2+size:], data[:2+size]
-        return data, b''  # no complete packet available yet
+        self.verbose = False  # TODO node
 
     @staticmethod
     def fix_range(value):
@@ -64,16 +39,11 @@ def fix_range(value):
             value -= 512
         return value
 
-    def process_packet(self, packet, verbose=False):
-        if packet == CAN_BRIDGE_READY:
-            self.bus.publish('can', CAN_BRIDGE_SYNC)
-            self.bus.publish('can', CAN_SPEED_1MB)
-            self.bus.publish('can', CAN_BRIDGE_START)
-            self.can_bridge_initialized = True
-            return None
-
-        if len(packet) >= 2:
-            msg_id = ((packet[0]) << 3) | (((packet[1]) >> 5) & 0x1f)
+    def process_packet(self, data, verbose=False):
+        msg_id, packet, flags = data
+        if True:  #len(packet) >= 2:
+#            msg_id = ((packet[0]) << 3) | (((packet[1]) >> 5) & 0x1f)
+            packet = b'XX' + packet  # hack for backward compatibility
             if verbose:
                 print(hex(msg_id), packet[2:])
             if msg_id == 0x200:
@@ -109,6 +79,12 @@ def process_packet(self, packet, verbose=False):
                 val = struct.unpack_from('HHBBH', packet, 2)
                 if verbose:
                     print("User:", val[2]&0x7F, val[3]&0x7F, val)
+            elif msg_id == 0x2A0:
+                # encoders
+                assert len(packet) == 2 + 4, packet
+                val = struct.unpack_from('hh', packet, 2)
+                if verbose:
+                    print("Enc:", val)
 
     def process_gen(self, data, verbose=False):
         self.buf, packet = self.split_buffer(self.buf + data)
@@ -122,11 +98,10 @@ def run(self):
         try:
             while True:
                 dt, channel, data = self.bus.listen()
-                if channel == 'raw':
-                    if len(data) > 0:
-                        for status in self.process_gen(data):
-                            if status is not None:
-                                self.bus.publish('status', status)
+                if channel == 'can':
+                    status = self.process_packet(data, verbose=self.verbose)
+                    if status is not None:
+                        self.bus.publish('status', status)
                 elif channel == 'move':
                     self.desired_speed, self.desired_angle = data
                 else:
@@ -138,7 +113,7 @@ def request_stop(self):
         self.bus.shutdown()
 
     def send(self, data):
-        if self.can_bridge_initialized:
+        if True:  #self.can_bridge_initialized:
             speed, angular_speed = data
             if speed > 0:
                 if self.status_word is None or self.status_word & 0x10 != 0:
@@ -159,15 +134,15 @@ def send(self, data):
                     else:
                         angle_cmd = 0
                 if speed >= 10:
-                    packet = CAN_packet(0x401, [0x80 + 127, angle_cmd])
+                    packet = CAN_triplet(0x401, [0x80 + 127, angle_cmd])
                 else:
-                    packet = CAN_packet(0x401, [0x80 + 80, angle_cmd])
+                    packet = CAN_triplet(0x401, [0x80 + 80, angle_cmd])
             else:
-                packet = CAN_packet(0x401, [0, 0])  # STOP packet
+                packet = CAN_triplet(0x401, [0, 0])  # STOP packet
             self.bus.publish('can', packet)
 
             # alive
-            packet = CAN_packet(0x400, [self.status_cmd, self.alive])
+            packet = CAN_triplet(0x400, [self.status_cmd, self.alive])
             self.bus.publish('can', packet)
             self.alive = 128 - self.alive
         else:

osgar/drivers/test_spider.py

@@ -2,42 +2,23 @@
 from unittest.mock import MagicMock
 from datetime import timedelta
 
-from osgar.drivers.spider import Spider, CAN_packet
+from osgar.drivers.spider import Spider, CAN_triplet
 from osgar.bus import Bus
 
-class SpiderTest(unittest.TestCase):
-
-    def test_split_buffer(self):
-        self.assertEqual(Spider.split_buffer(b''), (b'', b''))
-        self.assertEqual(Spider.split_buffer(b'\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xfe\x10'), (b'', b'\xfe\x10'))
-        data = b'\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xfeW\xfe0@h\x9e\x01i\x01\xf7\x01\x18\x00'
-        self.assertEqual(Spider.split_buffer(data), (b'\xfe0@h\x9e\x01i\x01\xf7\x01\x18\x00', b'\xfeW'))
-
-        data = b'0@h\x9e\x01i\x01\xf7\x01\x18\x00'
-        self.assertEqual(Spider.split_buffer(data), (b'h\x9e\x01i\x01\xf7\x01\x18\x00', b'0@'))
 
-    def test_can_packet(self):
-        self.assertEqual(CAN_packet(0x400, [0, 0]), b'\x80\x02\x00\x00')
-
-    def test_uninitialized_can_bridge(self):
-        bus = MagicMock()
-        spider = Spider(config={'stream_id_in':1, 'stream_id_out':2}, bus=bus)
-        spider.send((0, 0))
-#        bus.write.assert_called_once_with(0, 'ERROR: CAN bridge not initialized yet! [(0, 0)]')
+class SpiderTest(unittest.TestCase):
 
     def test_publish_status(self):
         logger=MagicMock()
         logger.write = MagicMock(return_value=timedelta(seconds=135))
         bus = Bus(logger=logger)
         tester = bus.handle('tester')
-        tester.register('raw')
+        tester.register('can')
         spider = Spider(config={}, bus=bus.handle('spider'))
-        bus.connect('tester.raw', 'spider.raw')
+        bus.connect('tester.can', 'spider.can')
         bus.connect('spider.status', 'tester.status')
 
-        spider.can_bridge_initialized = True  # skip initialization
-        self.assertEqual(CAN_packet(0x200, [0, 0x80]), b'@\x02\x00\x80')
-        tester.publish('raw', b'@\x02\x00\x80')
+        tester.publish('can', CAN_triplet(0x200, [0, 0x80]))
         spider.start()
         dt, stream, data = tester.listen()
         spider.request_stop()