ban-module/test/comm.py

import serial
import time
import struct

usleep = lambda x: time.sleep(x/1000000.0)

SET = 0x20
GET = 0x00


CMD_ECHO = 0x01
CMD_STOP = 0x02
CMD_START = 0x03
CMD_PARAM = 0x04
CMD_SENSOR_CNT = 0x05
CMD_SLEEP = 0x06
CMD_WAKEUP = 0x07
CMD_CALIBRATE = 0x08
CMD_RESET = 0x09
CMD_STORE = 0x0A
CMD_DATA = 0x0B
CMD_SYNC = 0x0C
CMD_SENSOR_TYPE = 0x0D
CMD_INFO = 0x0E

PARAM_NONE = 0xFF
PARAM_TIMEBASE = 0
PARAM_RESOLUTION = 1
PARAM_GAIN = 2 
PARAM_OFFSET = 3
PARAM_SAMPLERATE = 4
PARAM_RANGE = 5
PARAM_RANGE0 = 6
PARAM_FILTER = 7

PARAM_NAME = {
    PARAM_NONE : "NONE",
    PARAM_TIMEBASE : "TIMEBASE",
    PARAM_RESOLUTION : "RESOLUTION",
    PARAM_GAIN : "GAIN",
    PARAM_OFFSET : "OFFSET",
    PARAM_SAMPLERATE : "SAMPLERATE",
    PARAM_RANGE : "RANGE",
    PARAM_RANGE0 : "RANGE0",
    PARAM_FILTER : "FILTER",
}

crc8x_table = [
        0x00,0x07,0x0E,0x09,0x1C,0x1B,0x12,0x15,0x38,0x3F,0x36,0x31,0x24,0x23,0x2A,0x2D,
        0x70,0x77,0x7E,0x79,0x6C,0x6B,0x62,0x65,0x48,0x4F,0x46,0x41,0x54,0x53,0x5A,0x5D,
        0xE0,0xE7,0xEE,0xE9,0xFC,0xFB,0xF2,0xF5,0xD8,0xDF,0xD6,0xD1,0xC4,0xC3,0xCA,0xCD,
        0x90,0x97,0x9E,0x99,0x8C,0x8B,0x82,0x85,0xA8,0xAF,0xA6,0xA1,0xB4,0xB3,0xBA,0xBD,
        0xC7,0xC0,0xC9,0xCE,0xDB,0xDC,0xD5,0xD2,0xFF,0xF8,0xF1,0xF6,0xE3,0xE4,0xED,0xEA,
        0xB7,0xB0,0xB9,0xBE,0xAB,0xAC,0xA5,0xA2,0x8F,0x88,0x81,0x86,0x93,0x94,0x9D,0x9A,
        0x27,0x20,0x29,0x2E,0x3B,0x3C,0x35,0x32,0x1F,0x18,0x11,0x16,0x03,0x04,0x0D,0x0A,
        0x57,0x50,0x59,0x5E,0x4B,0x4C,0x45,0x42,0x6F,0x68,0x61,0x66,0x73,0x74,0x7D,0x7A,
        0x89,0x8E,0x87,0x80,0x95,0x92,0x9B,0x9C,0xB1,0xB6,0xBF,0xB8,0xAD,0xAA,0xA3,0xA4,
        0xF9,0xFE,0xF7,0xF0,0xE5,0xE2,0xEB,0xEC,0xC1,0xC6,0xCF,0xC8,0xDD,0xDA,0xD3,0xD4,
        0x69,0x6E,0x67,0x60,0x75,0x72,0x7B,0x7C,0x51,0x56,0x5F,0x58,0x4D,0x4A,0x43,0x44,
        0x19,0x1E,0x17,0x10,0x05,0x02,0x0B,0x0C,0x21,0x26,0x2F,0x28,0x3D,0x3A,0x33,0x34,
        0x4E,0x49,0x40,0x47,0x52,0x55,0x5C,0x5B,0x76,0x71,0x78,0x7F,0x6A,0x6D,0x64,0x63,
        0x3E,0x39,0x30,0x37,0x22,0x25,0x2C,0x2B,0x06,0x01,0x08,0x0F,0x1A,0x1D,0x14,0x13,
        0xAE,0xA9,0xA0,0xA7,0xB2,0xB5,0xBC,0xBB,0x96,0x91,0x98,0x9F,0x8A,0x8D,0x84,0x83,
        0xDE,0xD9,0xD0,0xD7,0xC2,0xC5,0xCC,0xCB,0xE6,0xE1,0xE8,0xEF,0xFA,0xFD,0xF4,0xF3
    ]


def crc8(data):
    crc = 0    
    length = len(data)
    if length ==0:
        return 0xff
    crc &= 0xff;
    i = 0
    while length>0:
        crc = crc8x_table[crc ^ data[i]];
        length=length-1
        i=i+1
    return crc;


def local_logger(*message):
    for i in range(len(message)):
        if (type(message[i]) == type([])):
            for m in message[i]:
                print(hex(m), end="|")
            continue
        print(message[i], end=" ")
    print()


class SerialComm:
    def __init__(self, port_name, enable_log = False):
        self.port = serial.Serial(timeout=0.05)
        if port_name != "":
            self.port.port = port_name
        else:
            self.port.port = '/dev/ttyACM0'

        self.port.parity = serial.PARITY_NONE
        self.port.baudrate = 921600
        self.comm_thread = None
        self.callback_call = local_logger
        self.measure_active = False
        self.data_file = None
        self.enable_logger = enable_log

    def callback(self, *message):
        if self.enable_logger:
            self.callback_call(*message)

    def open(self):
        self.port.open()
        self.port.flush()
        self.callback('i', 0, 'Open communication port')

    def close(self):
        self.port.close()

    def is_connected(self):
        return self.port.is_open

    def set_logger(self, callback):
        self.callback = callback

    def create_packet(self, start, data):
        if len(data) > 0:
            for c in data:
                start.append(c)

        if not self.port.is_open:
            self.open()
        self.port.flush()

        start[0] = len(start)
        crc_sum = crc8(start[1:])
        
        start.append(crc_sum)
        return start

    # def wait_to_response(self, extra_delay):
    #     communication_delay = 0.001
    #     t0 = time.time()  # timeout for response
    #     response_length = self.port.inWaiting()
    #     timeout = 0.05
    #     if extra_delay != 0:
    #         timeout = 0.5
    #     while response_length == 0:
    #         time.sleep(communication_delay)
    #         response_length = self.port.inWaiting()
    #         if time.time() - t0 > timeout:
    #             self.callback('d', 0, "SKIP response")
    #             return 0
    #     return 1

    def receive_response(self):
        data = []
        # time.sleep(0.005)
        response_l = self.port.read(1)

        if len(response_l) == 0:
            return []
        response = self.port.read(ord(response_l))
        for bajt in response:
            data.append(bajt)
        self.callback('d', 0, "\tRS>", data)
        return data

    def requestBroadcast(self, command, data):
        m = self.create_packet([0, 0, 0, command], data)
        self.callback('d', 0, "\tRQ BC>", m)
        # self.port.write([m[0]]) # send legth of message
        # self.port.write(m[1:]) # send body of message
        self.port.write(m) # send body of message

    def request(self, module, sensor, command, data, skip_check=False, long_answer=0):
        m = self.create_packet([0, module, sensor, command], data)
        self.callback('d', sensor, "\tRQ>", m)
        # SEND THE REQUEST
        # self.port.write(m)
        has_response = False
        counter = 0
        while has_response == False:
            # self.port.write([m[0]]) # send legth of message
            # self.port.write(m[1:]) # send body of message

            self.port.write(m) # send body of message
            if long_answer > 0:
                time.sleep(long_answer)
            data = self.receive_response()
            if len(data)>0:
                has_response = True
            counter = counter + 1
            if counter > 5:
                return []
        
        if len(data) == 0:
            return []

        if data[2] & 0x80 != 0:
            print("Chyba", hex(data[3]))
            return [data[3]]
        return data

    def read_bytes(self, num):
        return self.port.read(num)

    def prepare_read_data(self):
        if not self.port.is_open:
            self.open()
        self.flush()

def to_int16(bytes_data):
    uint16 = bytes_data[0] * 256 + bytes_data[1]
    if uint16 > 2 ** 15:
        uint16 = uint16 - 2 ** 16
    return uint16