From 92ffd7d81729bc0a786ad093bf7f77286ea101b0 Mon Sep 17 00:00:00 2001 From: Martin Dlouhy Date: Tue, 18 Aug 2020 19:34:32 +0200 Subject: [PATCH 1/4] config/test-spider.json - add separate module for CAN parsing --- config/test-spider.json | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/config/test-spider.json b/config/test-spider.json index ab91e62ff..1f3aa160e 100644 --- a/config/test-spider.json +++ b/config/test-spider.json @@ -4,11 +4,17 @@ "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"], @@ -16,7 +22,9 @@ "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"]] } } From d547b87c31a212c788ceb6a292fff00087c99e19 Mon Sep 17 00:00:00 2001 From: Martin Dlouhy Date: Tue, 18 Aug 2020 19:57:40 +0200 Subject: [PATCH 2/4] spider.py - refactor to use CAN stream instead of raw data --- osgar/drivers/spider.py | 65 ++++++++++-------------------------- osgar/drivers/test_spider.py | 29 +++------------- 2 files changed, 22 insertions(+), 72 deletions(-) diff --git a/osgar/drivers/spider.py b/osgar/drivers/spider.py index 206749c0e..e48edaea1 100644 --- a/osgar/drivers/spider.py +++ b/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: @@ -122,11 +92,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 +107,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 +128,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: diff --git a/osgar/drivers/test_spider.py b/osgar/drivers/test_spider.py index a58016582..e77084ca7 100644 --- a/osgar/drivers/test_spider.py +++ b/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() From 991d59229d2ba9ba60348c2855e1b4558904fff9 Mon Sep 17 00:00:00 2001 From: Martin Dlouhy Date: Tue, 1 Sep 2020 18:41:41 +0200 Subject: [PATCH 3/4] add config/spider.json --- config/spider.json | 40 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 40 insertions(+) create mode 100644 config/spider.json diff --git a/config/spider.json b/config/spider.json new file mode 100644 index 000000000..fb42b4680 --- /dev/null +++ b/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"]] + } +} From 808c96430c16c299789c0deaac6cf066196bf3b0 Mon Sep 17 00:00:00 2001 From: Martin Dlouhy Date: Tue, 1 Sep 2020 23:50:34 +0200 Subject: [PATCH 4/4] spider.py - print encoders --- osgar/drivers/spider.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/osgar/drivers/spider.py b/osgar/drivers/spider.py index e48edaea1..9e388ab41 100644 --- a/osgar/drivers/spider.py +++ b/osgar/drivers/spider.py @@ -79,6 +79,12 @@ def process_packet(self, data, 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)