Skip to content
This repository was archived by the owner on Jun 28, 2023. It is now read-only.
Open
Show file tree
Hide file tree
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Formating
  • Loading branch information
akrlfix committed Jan 5, 2022
commit 1e3bbcad46264c84adc65236e7e35d0a977408ab
40 changes: 25 additions & 15 deletions thingset/cansocket.py
Original file line number Diff line number Diff line change
@@ -1,7 +1,11 @@
import datetime, socket, struct, time
import datetime
import socket
import struct
import time
from cbor2 import loads
from thingset.packet import TSPacket, SingleFrame, RequestFrame


class CANsocket(object):
FMT = '<IB3x8s'

Expand Down Expand Up @@ -34,14 +38,15 @@ class CANsocket(object):

fc_flags = {0x00: 'Continue', 0x01: 'Wait', 0x02: 'Abort'}

def __init__(self, interface:str, address:int=0x01):
def __init__(self, interface: str, address: int = 0x01):
self.s = socket.socket(socket.PF_CAN, socket.SOCK_RAW, socket.CAN_RAW)
self.s.bind((interface,))
self.address = address

def subscribe(self, addr:int) -> None:
if addr not in range(0,256):
raise ValueError("Address must be integer between 0 and 255 (got: {})".format(addr))
def subscribe(self, addr: int) -> None:
if addr not in range(0, 256):
raise ValueError(
"Address must be integer between 0 and 255 (got: {})".format(addr))
if not addr in self.sub_addresses:
self.sub_addresses.append(addr)

Expand All @@ -57,7 +62,7 @@ def receive(self) -> tuple:
packet = self.s.recv(64)
can_id, length, data = struct.unpack(self.FMT, packet)
can_id &= socket.CAN_EFF_MASK

if (can_id & TSPacket.TS_FRAME_FLAG):
frame = SingleFrame(data=data)
frame.parseIdentifier(can_id)
Expand All @@ -78,7 +83,8 @@ def receive(self) -> tuple:
self.databuffer.clear()
self.databuffer.extend(frame.data[2:])
self.last_index = 0
fc_frame = RequestFrame(src=self.address, dst=frame.source, data=b'\x30\x00\x00')
fc_frame = RequestFrame(
src=self.address, dst=frame.source, data=b'\x30\x00\x00')
self.send(fc_frame)
if frame.type == RequestFrame.FRAME_TYPE_CONSEC:
expected_index = (self.last_index + 1) % 16
Expand All @@ -88,30 +94,33 @@ def receive(self) -> tuple:
self.databuffer.extend(frame.data)
if len(self.databuffer) >= self.packetsize:
status = self.status_code[self.databuffer[0]]
ret = (False, status, (loads(self.databuffer[1:])), None)
ret = (False, status,
(loads(self.databuffer[1:])), None)
if frame.type == RequestFrame.FRAME_TYPE_FLOWC:
self.flow_control['flag'] = frame.fcflag
self.flow_control['blocksize'] = frame.fcflag
self.flow_control['delay'] = frame.fcflag

return ret

def send(self, message:RequestFrame) -> bool:
def send(self, message: RequestFrame) -> bool:
'''Send function with basic ThingSet/ISO-TP handling
Returns True on Success
'''
can_id = message.identifier | socket.CAN_EFF_FLAG

if message.type == RequestFrame.FRAME_TYPE_FLOWC:
can_packet = struct.pack(self.FMT, can_id, len(message.data), message.data)
can_packet = struct.pack(
self.FMT, can_id, len(message.data), message.data)
self.s.send(can_packet)
else:
if len(message.data) <= 7:
# data fits in single frame
frame_data = bytearray()
frame_data.append(len(message.data))
frame_data.extend(message.data)
can_packet = struct.pack(self.FMT, can_id, len(frame_data), frame_data)
can_packet = struct.pack(
self.FMT, can_id, len(frame_data), frame_data)
self.s.send(can_packet)
else:
# First Frame
Expand Down Expand Up @@ -141,7 +150,8 @@ def send(self, message:RequestFrame) -> bool:
frame_data.append(0x20 | index)
frame_data.extend(data[:7])
del data[:7]
self.s.send(struct.pack(self.FMT, can_id, len(frame_data), frame_data))
self.s.send(struct.pack(
self.FMT, can_id, len(frame_data), frame_data))
block += 1
if block == blocksize:
block = 0
Expand All @@ -160,15 +170,15 @@ def send(self, message:RequestFrame) -> bool:
index += 1
frame_data.append(0x20 | index)
frame_data.extend(data)
self.s.send(struct.pack(self.FMT, can_id, len(frame_data), frame_data))
self.s.send(struct.pack(
self.FMT, can_id, len(frame_data), frame_data))
del data[:]
else:
print("Flow Control Timeout before sending")
return False
return True


def waitFC(self, timeout:float=0.5):
def waitFC(self, timeout: float = 0.5):
'''Wait for Flow Control Frame and return False on Timeout and True on Success'''
ret = bool(False)
timeout = datetime.datetime.now() + datetime.timedelta(seconds=timeout)
Expand Down
47 changes: 30 additions & 17 deletions thingset/packet.py
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
from socket import CAN_EFF_FLAG
from cbor2 import loads


class TSPacket(object):
TS_FRAME_FLAG = (1 << 24)

Expand All @@ -14,8 +15,9 @@ def source(self):

@source.setter
def source(self, source):
if source not in range(0,256):
raise ValueError("Source ID must be integer between 0 and 255 (got: {})".format(source))
if source not in range(0, 256):
raise ValueError(
"Source ID must be integer between 0 and 255 (got: {})".format(source))
self._source = source

@property
Expand All @@ -25,7 +27,8 @@ def timestamp(self):
@timestamp.setter
def timestamp(self, timestamp):
if not isinstance(timestamp, float):
raise TypeError("Timestamp must be float (got: {})".format(type(timestamp)))
raise TypeError(
"Timestamp must be float (got: {})".format(type(timestamp)))
self._timestamp = timestamp


Expand All @@ -46,8 +49,9 @@ def dataobjectID(self):

@dataobjectID.setter
def dataobjectID(self, dataobjectID):
if not dataobjectID in range(0,65537):
raise ValueError("Data object ID must be integer between 0 and 65536 (got: {}).".format(dataobjectID))
if not dataobjectID in range(0, 65537):
raise ValueError(
"Data object ID must be integer between 0 and 65536 (got: {}).".format(dataobjectID))
self._dataobjectID = dataobjectID


Expand All @@ -65,9 +69,11 @@ def __init__(self, data=None, dataobjectID=0, priority=6, source=0, timestamp=0.

def parseIdentifier(self, identifier):
if not isinstance(identifier, int):
raise ValueError("Identifier must be integer, not {}.".format(identifier))
raise ValueError(
"Identifier must be integer, not {}.".format(identifier))
if identifier >= (1 << 30):
raise ValueError("Identifier too big. Cannot contain more than 29 bits")
raise ValueError(
"Identifier too big. Cannot contain more than 29 bits")
if not (identifier & TSPacket.TS_FRAME_FLAG):
raise ValueError("Not a publication message.")
self.priority = identifier >> 26
Expand Down Expand Up @@ -97,14 +103,16 @@ def data(self, data):
if data is None:
self._data = bytes()
elif not isinstance(data, bytes):
raise TypeError("Wrong data type. Must be bytes, not {}".format(type(data)))
raise TypeError(
"Wrong data type. Must be bytes, not {}".format(type(data)))
self._data = data
self._cbor = loads(self._data)

@property
def cbor(self):
return self._cbor


class ServiceFrame(TSPacket):
def __init__(self, priority=7, destination=0, source=0):
super().__init__()
Expand All @@ -123,8 +131,9 @@ def priority(self):

@priority.setter
def priority(self, priority):
if priority not in range(0,8):
raise ValueError("Priority must be integer between 0 and 7 (got: {})".format(priority))
if priority not in range(0, 8):
raise ValueError(
"Priority must be integer between 0 and 7 (got: {})".format(priority))
self._priority = priority

@property
Expand All @@ -133,10 +142,12 @@ def destination(self):

@destination.setter
def destination(self, destination):
if destination not in range(0,256):
raise ValueError("Destination ID must be integer between 0 and 255 (got: {})".format(destination))
if destination not in range(0, 256):
raise ValueError(
"Destination ID must be integer between 0 and 255 (got: {})".format(destination))
self._destination = destination


class RequestFrame(ServiceFrame):
SINGLE_ID_MASK = (0b10 << 24)

Expand Down Expand Up @@ -164,12 +175,13 @@ def __init__(self, priority=7, src=0, dst=0, data=bytes()):
self.blocksize = data[1]
self.delay = data[2]


def parseIdentifier(self, identifier):
if not isinstance(identifier, int):
raise ValueError("Identifier must be integer, not {}.".format(identifier))
raise ValueError(
"Identifier must be integer, not {}.".format(identifier))
if identifier >= (1 << 30):
raise ValueError("Identifier too big. Cannot contain more than 29 bits")
raise ValueError(
"Identifier too big. Cannot contain more than 29 bits")
if (identifier & TSPacket.TS_FRAME_FLAG):
raise ValueError("Not a request/response message.")
self.priority = identifier >> 26
Expand All @@ -190,7 +202,7 @@ def type(self):
@type.setter
def type(self, type):
self._type = type

@property
def data(self):
return self._data
Expand All @@ -200,5 +212,6 @@ def data(self, data):
if data is None:
self._data = bytes()
elif not isinstance(data, bytes):
raise TypeError("Wrong data type. Must be bytes, not {}".format(type(data)))
raise TypeError(
"Wrong data type. Must be bytes, not {}".format(type(data)))
self._data = data
Loading