107 lines
3.1 KiB
Python
107 lines
3.1 KiB
Python
"""
|
|
NMEA2000 Nachricht
|
|
|
|
"""
|
|
|
|
import can
|
|
import struct
|
|
from math import ceil
|
|
from .pgntype import pgntype
|
|
|
|
class Message():
|
|
|
|
def __init__(self, bus, source, data=None):
|
|
self._bus = bus
|
|
self._source = source
|
|
self._priority = 5
|
|
if data:
|
|
print("msg: set data: ", type(data))
|
|
self._data = data
|
|
else:
|
|
self._data = bytearray()
|
|
self.pgn = None
|
|
#self.sequence = 0xff
|
|
self.sequence = 0
|
|
|
|
def set_pgn(self, pgn):
|
|
self.pgn = pgn
|
|
|
|
def set_priority(self, priority):
|
|
self._priority = priority
|
|
|
|
def get_fast_frames(self, rawdata, sc):
|
|
# Erstelle ein Liste von Frames aufgrund vorliegender Rohdaten
|
|
# zum transferieren von bis zu 223 Bytes über eine fast-Message
|
|
# Der sequence-counter muß extern inkrementiert werden, er hat
|
|
# 3 Bit Länge. Der Framecounter fc hat eine Länge von 5 Bit.
|
|
# Der erste Frame kann 6 Bytes aufnehmen
|
|
# Alle weiteren Frames jeweils 7 Bytes
|
|
datalen = len(rawdata)
|
|
if datalen > 223:
|
|
raise ValueError("data for fast packet too long")
|
|
if datalen < 7:
|
|
raise ValueError("data for fast packet too short")
|
|
nf = ceil((datalen - 6) / 7 + 1) # number of frames
|
|
fc = 0 # frame counter, nibble 2
|
|
frames = list()
|
|
# Frame 1
|
|
data = bytearray()
|
|
data.append((sc << 5) + fc)
|
|
data.append(datalen)
|
|
for b in rawdata[:6]:
|
|
data.append(b)
|
|
frames.append(data)
|
|
nf -= 1
|
|
# Frame 2..n
|
|
p = 6
|
|
while nf > 0:
|
|
data = bytearray()
|
|
fc += 1
|
|
data.append((sc << 5) + fc)
|
|
for b in rawdata[p:p+7]:
|
|
data.append(b)
|
|
frames.append(data)
|
|
p += 7
|
|
nf -= 1
|
|
print("message:get_fast_frames")
|
|
print(frames)
|
|
return frames
|
|
|
|
def send_single(self):
|
|
msg = can.Message(
|
|
arbitration_id = (((self._priority << 18) + self.pgn) << 8) + self._source,
|
|
data = self._data,
|
|
is_extended_id = True
|
|
);
|
|
try:
|
|
self._bus.send(msg)
|
|
except can.CanError:
|
|
print(f"Message {self.pgn} NOT sent")
|
|
return False
|
|
return True
|
|
|
|
def send_fast(self):
|
|
id = (((self._priority << 18) + self.pgn) << 8) + self._source
|
|
try:
|
|
for frame in self.get_fast_frames(self._data, self.sequence):
|
|
msg = can.Message(
|
|
arbitration_id = id,
|
|
data = frame,
|
|
is_extended_id = True
|
|
);
|
|
self._bus.send(msg)
|
|
except can.CanError:
|
|
print(f"Message {self.pgn} NOT sent")
|
|
return False
|
|
finally:
|
|
self.sequence += 1
|
|
self.sequence %= 8
|
|
print("send fast: adjusted sequence {}".format(self.sequence))
|
|
return True
|
|
|
|
def send(self):
|
|
if pgntype(self.pgn) == "S":
|
|
self.send_single()
|
|
else:
|
|
self.send_fast()
|