Fix frame decoding

setup.py

@@ -84,6 +84,7 @@
 
     packages = find_packages("src"),
     package_dir = {"": "src"},
+    package_data = {"canmatrix" : ["tests/*.dbc"]},
     entry_points={'console_scripts': ['cancompare = canmatrix.compare:main',
                                       'canconvert = canmatrix.convert:main']}
 )

src/canmatrix/canmatrix.py

@@ -30,7 +30,7 @@
 from __future__ import division
 import math
 import attr
-
+import sys
 if attr.__version__ < '17.4.0':
     raise "need attrs >= 17.4.0"
 
@@ -44,6 +44,12 @@
 
 
 logger = logging.getLogger('root')
+try:
+    from itertools import zip_longest as zip_longest
+except:
+    from itertools import izip_longest as zip_longest
+from itertools import chain
+import struct
 import bitstruct
 
 from past.builtins import basestring
@@ -342,6 +348,62 @@ def bitstruct_format(self):
 
         return endian + bit_type + str(self.size)
 
+    def unpack_bitstring(self, length, is_float, is_signed, bits):
+        if is_float:
+            types = {
+                32: '>f',
+                64: '>d'
+            }
+
+            float_type = types.get(length)
+
+            if float_type is None:
+                raise Exception(
+                    'float type only supports lengths in [{}]'.
+                        format(', '.join([str(t) for t in types.keys()]))
+                )
+            if sys.version_info > (3, 0):
+                value, = struct.unpack(float_type, bytes(int(''.join(b), 2)  for b in grouper(bits, 8)))
+            else:
+                value, = struct.unpack(float_type, bytearray(int(''.join(b), 2)  for b in grouper(bits, 8)))
+
+        else:
+            value = int(bits, 2)
+
+            if is_signed and bits[0] == '1':
+                value -= (1 << len(bits))
+
+        return value
+
+    def pack_bitstring(self, length, is_float, value, signed):
+        if is_float:
+            types = {
+                32: '>f',
+                64: '>d'
+            }
+
+            float_type = types.get(length)
+
+            if float_type is None:
+                raise Exception(
+                    'float type only supports lengths in [{}]'.
+                        format(', '.join([str(t) for t in types.keys()]))
+                )
+
+            x = struct.pack(float_type, value)
+
+            if sys.version_info < (3, 0):
+                x = map(ord, tuple(x))
+            bitstring = ''.join('{:08b}'.format(b) for b in x)
+        else:
+            b = value.to_bytes(math.ceil(length / 8), byteorder='big',
+                               signed=signed)
+            b = '{:0{}b}'.format(int.from_bytes(b, byteorder='big'),
+                                 length)
+            bitstring = b[-length:]
+
+        return bitstring
+
     def phys2raw(self, value=None):
         """Return the raw value (= as is on CAN).
 
@@ -445,6 +507,29 @@ def __getitem__(self, name):
         raise KeyError("Signal '{}' doesn't exist".format(name))
 
 
+
+class decodedSignal(object):
+    def __init__(self, rawValue, signal):
+        self.rawValue = rawValue
+        self.signal = signal
+
+    @property
+    def physValue(self):
+        return self.signal.raw2phys(self.rawValue)
+
+    @property
+    def namedValue(self):
+        return self.signal.raw2phys(self.rawValue, decodeToStr=True)
+
+
+# https://docs.python.org/3/library/itertools.html
+def grouper(iterable, n, fillvalue=None):
+    "Collect data into fixed-length chunks or blocks"
+    # grouper('ABCDEFG', 3, 'x') --> ABC DEF Gxx"
+    args = [iter(iterable)] * n
+    return zip_longest(*args, fillvalue=fillvalue)
+
+
 @attr.s(cmp=False)
 class Frame(object):
     """
@@ -764,11 +849,6 @@ def createDummySignals(self):
                 startBit = -1
                 sigCount +=1
 
-        # bitfield = self.findNotUsedBits()
-        # for i in range(0,8):
-        #    print (bitfield[(i)*8:(i+1)*8])
-
-
 
 
     def updateReceiver(self):
@@ -779,31 +859,43 @@ def updateReceiver(self):
             for receiver in sig.receiver:
                 self.addReceiver(receiver)
 
-    def bitstruct_format(self, signalsToDecode = None):
-        """Returns the Bitstruct format string of this frame
 
-        :return: Bitstruct format string.
-        :rtype: str
-        """
-        fmt = []
-        frame_size = self.size * 8
-        end = frame_size
-        if signalsToDecode is None:
-            signals = sorted(self.signals, key=lambda s: s.getStartbit())
+    def signals_to_bytes(self, data):
+        little_bits = [None] * (self.size * 8)
+        big_bits = list(little_bits)
+        for signal in self.signals:
+            if signal.name in data:
+                value = data.get(signal.name)
+                bits = signal.pack_bitstring(signal.size, signal.is_float, value, signal.is_signed)
+
+                if signal.is_little_endian:
+                    least = self.size * 8 - signal.startBit
+                    most = least - signal.size
+
+                    little_bits[most:least] = bits
+                else:
+                    most = signal.startBit
+                    least = most + signal.size
+
+                    big_bits[most:least] = bits
+        little_bits = reversed(tuple(grouper(little_bits, 8)))
+        little_bits = tuple(chain(*little_bits))
+        bitstring = ''.join(
+            next(x for x in (l, b, '0') if x is not None)
+            # l if l != ' ' else (b if b != ' ' else '0')
+            for l, b in zip(little_bits, big_bits)
+        )
+        if sys.version_info > (3, 0):
+            return bytes(
+                int(''.join(b), 2)
+                for b in grouper(bitstring, 8)
+            )
         else:
-            signals = sorted(signalsToDecode, key=lambda s: s.getStartbit())
+            return bytearray(
+                int(''.join(b), 2)
+                for b in grouper(bitstring, 8)
+            )
 
-        for signal in signals:
-            start = frame_size - signal.getStartbit() - signal.size
-            padding = end - (start + signal.size)
-            if padding > 0:
-                fmt.append('p' + str(padding))
-            fmt.append(signal.bitstruct_format())
-            end = start
-        if end != 0:
-            fmt.append('p' + str(end))
-        # assert bitstruct.calcsize(''.join(fmt)) == frame_size
-        return ''.join(fmt)
 
     def encode(self, data=None):
         """Return a byte string containing the values from data packed
@@ -813,74 +905,110 @@ def encode(self, data=None):
         :return: A byte string of the packed values.
         :rtype: bitstruct
         """
+
+
         data = dict() if data is None else data
 
         if self.is_complex_multiplexed:
             # TODO
-            pass
+            Exception("encoding complex multiplexed frames not yet implemented")
         elif self.is_multiplexed:
             # search for mulitplexer-signal
             muxSignal = None
             for signal in self.signals:
                 if signal.is_multiplexer:
                     muxSignal = signal
                     muxVal = data.get(signal.name)
+                    break
             # create list of signals which belong to muxgroup
-            encodeSignals = [muxSignal]
+            encodeSignals = [muxSignal.name]
             for signal in self.signals:
                 if signal.mux_val == muxVal or signal.mux_val is None:
-                    encodeSignals.append(signal)
-            fmt = self.bitstruct_format(encodeSignals)
-            signals = sorted(encodeSignals, key=lambda s: s.getStartbit())
-        else:
-            fmt = self.bitstruct_format()
-            signals = sorted(self.signals, key=lambda s: s.getStartbit())
-        signal_value = []
+                    encodeSignals.append(signal.name)
+            newData = dict()
+            # kick out signals, which do not belong to this mux-id
+            for signalName in data:
+                if signalName in encodeSignals:
+                    newData[signalName] = data[signalName]
+            data = newData
+        return self.signals_to_bytes(data)
+
+    def bytes_to_bitstrings(self, data):
+        b = tuple('{:08b}'.format(b) for b in data)
+        little = ''.join(reversed(b))
+        big = ''.join(b)
+
+        return little, big
+
+    def bitstring_to_signal_list(self, signals, big, little):
+        unpacked = []
         for signal in signals:
-            signal_value.append(
-                signal.phys2raw(data.get(signal.name))
-            )
+            if signal.is_little_endian:
+                least = self.size * 8 - signal.startBit
+                most = least - signal.size
+
+                bits = little[most:least]
+            else:
+                most = signal.startBit
+                least = most + signal.size
+
+                bits = big[most:least]
 
-        return bitstruct.pack(fmt, *signal_value)
+            unpacked.append(signal.unpack_bitstring(signal.size, signal.is_float, signal.is_signed, bits))
+
+        return unpacked
+
+    def unpack(self, data, report_error=True):
+        rx_length = len(data)
+        if rx_length != self.size and report_error:
+            print(
+                'Received message 0x{self.id:08X} with length {rx_length}, expected {self.size}'.format(**locals()))
+        else:
+            if sys.version_info > (3, 0):
+                little, big = self.bytes_to_bitstrings(bytes(data))
+            else:
+                little, big = self.bytes_to_bitstrings(data)
 
-    def decode(self, data, decodeToStr=False):
+            unpacked = self.bitstring_to_signal_list(self.signals, big, little)
+
+            returnDict= dict()
+
+            for s, v in zip(self.signals, unpacked):
+                returnDict[s.name] = decodedSignal(v, s)
+
+            return returnDict
+
+    ###
+
+    def decode(self, data):
         """Return OrderedDictionary with Signal Name: Signal Value
 
         :param data: Iterable or bytes.
             i.e. (0xA1, 0xA2, 0xA3, 0xA4, 0xA5, 0xA6, 0xA7, 0xA8)
         :param bool decodeToStr: If True, try to get value representation as *string* ('Init' etc.)
         :return: OrderedDictionary
         """
+        decoded = self.unpack(data)
+
         if self.is_complex_multiplexed:
             # TODO
-            pass
+            raise Exception("decoding complex multiplexed frames not yet implemented")
         elif self.is_multiplexed:
+            returnDict = dict()
             # find multiplexer and decode only its value:
+
             for signal in self.signals:
                 if signal.is_multiplexer:
-                    fmt = self.bitstruct_format([signal])
-                    signals = sorted(self.signals, key=lambda s: s.getStartbit())
-                    signals_values = OrderedDict()
-                    for signal, value in zip(signals, bitstruct.unpack(fmt, data)):
-                        signals_values[signal.name] = signal.raw2phys(value, decodeToStr)
-                muxVal = int(list(signals_values.values())[0])
+                    muxVal = decoded[signal.name].rawValue
+
             # find all signals with the identified multiplexer-value
-            muxedSignals = []
+
             for signal in self.signals:
                 if signal.mux_val == muxVal or signal.mux_val is None:
-                    muxedSignals.append(signal)
-            fmt = self.bitstruct_format(muxedSignals)
-            signals = sorted(muxedSignals, key=lambda s: s.getStartbit())
+                    returnDict[signal.name] = decoded[signal.name]
+            return returnDict
         else:
-            fmt = self.bitstruct_format()
-            signals = sorted(self.signals, key=lambda s: s.getStartbit())
-
-        # decode
-        signals_values = OrderedDict()
-        for signal, value in zip(signals, bitstruct.unpack(fmt, data)):
-            signals_values[signal.name] = signal.raw2phys(value, decodeToStr)
-
-        return signals_values
+            return decoded
 
 
     def __str__(self):
@@ -1563,53 +1691,6 @@ def EnumAttribs2Keys(self):
                             signal.attributes[define] = str(signal.attributes[define])
 
 
-def computeSignalValueInFrame(startBit, ln, fmt, value):
-    """
-    Compute the Signal value in the Frame.
-
-    :param int startBit: signal start bit
-    :param int ln: signal length
-    :param int fmt: byte order, 1 for Little Endian (Intel), else Big Endian (Motorola)
-    :param int value: signal value
-    :return: data frame with signal set to value
-    :rtype: int
-    """
-
-    frame = 0
-    if fmt == 1:  # Intel
-        # using "sawtooth bit counting policy" here
-        pos = ((7 - (startBit % 8)) + 8*(int(startBit/8)))
-        while ln > 0:
-            # How many bits can we stuff in current byte?
-            # (Should be 8 for anything but the first loop)
-            availbitsInByte = 1 + (pos % 8)
-            # extract relevant bits from value
-            valueInByte = value & ((1<<availbitsInByte)-1)
-            # stuff relevant bits into frame at the "corresponding inverted bit"
-            posInFrame = ((7 - (pos % 8)) + 8*(int(pos/8)))
-            frame |= valueInByte << posInFrame
-            # move to the next byte
-            pos += 0xf
-            # discard used bytes
-            value = value >> availbitsInByte
-            # reduce length by how many bits we consumed
-            ln -= availbitsInByte
-
-    else:  # Motorola
-        # Work this out in "sequential bit counting policy"
-        # Compute the LSB position in "sequential"
-        lsbpos = ((7 - (startBit % 8)) + 8*(int(startBit/8)))
-        # deduce the MSB position
-        msbpos = 1 + lsbpos - ln
-        # "reverse" the value
-        cvalue = int(format(value, 'b')[::-1],2)
-        # shift the value to the proper position in the frame
-        frame = cvalue << msbpos
-
-    # Return frame, to be accumulated by caller
-    return frame
-
-
 class CanId(object):
     """
     Split Id into Global source addresses (source, destination) off ECU and PGN (SAE J1939).