mirror of
https://github.com/openmv/openmv.git
synced 2025-09-26 23:09:13 +08:00
367 lines
9.6 KiB
Python
367 lines
9.6 KiB
Python
import struct
|
|
|
|
|
|
class ModbusRTU:
|
|
def __init__(self, uart, slave_id=0x01, register_num=30):
|
|
self.SLAVE_ID = slave_id
|
|
self.uart = uart
|
|
self.register_num = register_num
|
|
self.REGISTER = [0] * self.register_num
|
|
self.CRC16_TABLE = [
|
|
0x0000,
|
|
0xC0C1,
|
|
0xC181,
|
|
0x0140,
|
|
0xC301,
|
|
0x03C0,
|
|
0x0280,
|
|
0xC241,
|
|
0xC601,
|
|
0x06C0,
|
|
0x0780,
|
|
0xC741,
|
|
0x0500,
|
|
0xC5C1,
|
|
0xC481,
|
|
0x0440,
|
|
0xCC01,
|
|
0x0CC0,
|
|
0x0D80,
|
|
0xCD41,
|
|
0x0F00,
|
|
0xCFC1,
|
|
0xCE81,
|
|
0x0E40,
|
|
0x0A00,
|
|
0xCAC1,
|
|
0xCB81,
|
|
0x0B40,
|
|
0xC901,
|
|
0x09C0,
|
|
0x0880,
|
|
0xC841,
|
|
0xD801,
|
|
0x18C0,
|
|
0x1980,
|
|
0xD941,
|
|
0x1B00,
|
|
0xDBC1,
|
|
0xDA81,
|
|
0x1A40,
|
|
0x1E00,
|
|
0xDEC1,
|
|
0xDF81,
|
|
0x1F40,
|
|
0xDD01,
|
|
0x1DC0,
|
|
0x1C80,
|
|
0xDC41,
|
|
0x1400,
|
|
0xD4C1,
|
|
0xD581,
|
|
0x1540,
|
|
0xD701,
|
|
0x17C0,
|
|
0x1680,
|
|
0xD641,
|
|
0xD201,
|
|
0x12C0,
|
|
0x1380,
|
|
0xD341,
|
|
0x1100,
|
|
0xD1C1,
|
|
0xD081,
|
|
0x1040,
|
|
0xF001,
|
|
0x30C0,
|
|
0x3180,
|
|
0xF141,
|
|
0x3300,
|
|
0xF3C1,
|
|
0xF281,
|
|
0x3240,
|
|
0x3600,
|
|
0xF6C1,
|
|
0xF781,
|
|
0x3740,
|
|
0xF501,
|
|
0x35C0,
|
|
0x3480,
|
|
0xF441,
|
|
0x3C00,
|
|
0xFCC1,
|
|
0xFD81,
|
|
0x3D40,
|
|
0xFF01,
|
|
0x3FC0,
|
|
0x3E80,
|
|
0xFE41,
|
|
0xFA01,
|
|
0x3AC0,
|
|
0x3B80,
|
|
0xFB41,
|
|
0x3900,
|
|
0xF9C1,
|
|
0xF881,
|
|
0x3840,
|
|
0x2800,
|
|
0xE8C1,
|
|
0xE981,
|
|
0x2940,
|
|
0xEB01,
|
|
0x2BC0,
|
|
0x2A80,
|
|
0xEA41,
|
|
0xEE01,
|
|
0x2EC0,
|
|
0x2F80,
|
|
0xEF41,
|
|
0x2D00,
|
|
0xEDC1,
|
|
0xEC81,
|
|
0x2C40,
|
|
0xE401,
|
|
0x24C0,
|
|
0x2580,
|
|
0xE541,
|
|
0x2700,
|
|
0xE7C1,
|
|
0xE681,
|
|
0x2640,
|
|
0x2200,
|
|
0xE2C1,
|
|
0xE381,
|
|
0x2340,
|
|
0xE101,
|
|
0x21C0,
|
|
0x2080,
|
|
0xE041,
|
|
0xA001,
|
|
0x60C0,
|
|
0x6180,
|
|
0xA141,
|
|
0x6300,
|
|
0xA3C1,
|
|
0xA281,
|
|
0x6240,
|
|
0x6600,
|
|
0xA6C1,
|
|
0xA781,
|
|
0x6740,
|
|
0xA501,
|
|
0x65C0,
|
|
0x6480,
|
|
0xA441,
|
|
0x6C00,
|
|
0xACC1,
|
|
0xAD81,
|
|
0x6D40,
|
|
0xAF01,
|
|
0x6FC0,
|
|
0x6E80,
|
|
0xAE41,
|
|
0xAA01,
|
|
0x6AC0,
|
|
0x6B80,
|
|
0xAB41,
|
|
0x6900,
|
|
0xA9C1,
|
|
0xA881,
|
|
0x6840,
|
|
0x7800,
|
|
0xB8C1,
|
|
0xB981,
|
|
0x7940,
|
|
0xBB01,
|
|
0x7BC0,
|
|
0x7A80,
|
|
0xBA41,
|
|
0xBE01,
|
|
0x7EC0,
|
|
0x7F80,
|
|
0xBF41,
|
|
0x7D00,
|
|
0xBDC1,
|
|
0xBC81,
|
|
0x7C40,
|
|
0xB401,
|
|
0x74C0,
|
|
0x7580,
|
|
0xB541,
|
|
0x7700,
|
|
0xB7C1,
|
|
0xB681,
|
|
0x7640,
|
|
0x7200,
|
|
0xB2C1,
|
|
0xB381,
|
|
0x7340,
|
|
0xB101,
|
|
0x71C0,
|
|
0x7080,
|
|
0xB041,
|
|
0x5000,
|
|
0x90C1,
|
|
0x9181,
|
|
0x5140,
|
|
0x9301,
|
|
0x53C0,
|
|
0x5280,
|
|
0x9241,
|
|
0x9601,
|
|
0x56C0,
|
|
0x5780,
|
|
0x9741,
|
|
0x5500,
|
|
0x95C1,
|
|
0x9481,
|
|
0x5440,
|
|
0x9C01,
|
|
0x5CC0,
|
|
0x5D80,
|
|
0x9D41,
|
|
0x5F00,
|
|
0x9FC1,
|
|
0x9E81,
|
|
0x5E40,
|
|
0x5A00,
|
|
0x9AC1,
|
|
0x9B81,
|
|
0x5B40,
|
|
0x9901,
|
|
0x59C0,
|
|
0x5880,
|
|
0x9841,
|
|
0x8801,
|
|
0x48C0,
|
|
0x4980,
|
|
0x8941,
|
|
0x4B00,
|
|
0x8BC1,
|
|
0x8A81,
|
|
0x4A40,
|
|
0x4E00,
|
|
0x8EC1,
|
|
0x8F81,
|
|
0x4F40,
|
|
0x8D01,
|
|
0x4DC0,
|
|
0x4C80,
|
|
0x8C41,
|
|
0x4400,
|
|
0x84C1,
|
|
0x8581,
|
|
0x4540,
|
|
0x8701,
|
|
0x47C0,
|
|
0x4680,
|
|
0x8641,
|
|
0x8201,
|
|
0x42C0,
|
|
0x4380,
|
|
0x8341,
|
|
0x4100,
|
|
0x81C1,
|
|
0x8081,
|
|
0x4040,
|
|
]
|
|
|
|
def any(self):
|
|
return self.uart.any()
|
|
|
|
def clear(self):
|
|
self.REGISTER = [0] * self.register_num
|
|
|
|
def crc16(self, data):
|
|
crc = 0xFFFF
|
|
for char in data:
|
|
crc = (crc >> 8) ^ self.CRC16_TABLE[((crc) ^ char) & 0xFF]
|
|
return struct.pack("<H", crc)
|
|
|
|
def handle(self, debug=False):
|
|
REQUEST = self.uart.read()
|
|
if debug:
|
|
print("GOT REQUEST: ", REQUEST)
|
|
additional_address = REQUEST[0]
|
|
error_check = REQUEST[-2:]
|
|
function_code = REQUEST[1]
|
|
data = REQUEST[2:]
|
|
RESPONSE = struct.pack("b", self.SLAVE_ID)
|
|
if self.crc16(REQUEST[:-2]) != error_check:
|
|
if debug:
|
|
print("crc not match")
|
|
return 0 # do nothing
|
|
if self.SLAVE_ID != additional_address:
|
|
if debug:
|
|
print("OpenMV slave id: ", self.SLAVE_ID)
|
|
print("got cmd id: ", additional_address)
|
|
return 0 # do nothing
|
|
if function_code == 0x03:
|
|
starting_address = struct.unpack(">h", data[:2])[0]
|
|
quantity_of_registers = struct.unpack(">h", data[2:4])[0]
|
|
response_registers = []
|
|
try:
|
|
for i in range(starting_address, starting_address + quantity_of_registers):
|
|
response_registers.append(self.REGISTER[i])
|
|
except IndexError as err:
|
|
RESPONSE += struct.pack("b", function_code | 0x80)
|
|
RESPONSE += struct.pack("b", 0x02) # Illegal Data Address
|
|
if debug:
|
|
print("Illegal Data Address: ")
|
|
print(err)
|
|
else:
|
|
RESPONSE += struct.pack("b", function_code)
|
|
RESPONSE += struct.pack("b", 2 * quantity_of_registers)
|
|
for b in response_registers:
|
|
RESPONSE += struct.pack(">h", b)
|
|
elif function_code == 0x06:
|
|
register_address = struct.unpack(">h", data[:2])[0]
|
|
register_value = struct.unpack(">h", data[2:4])[0]
|
|
try:
|
|
self.REGISTER[register_address] = register_value
|
|
except IndexError as err:
|
|
RESPONSE += struct.pack("b", function_code | 0x80)
|
|
RESPONSE += struct.pack("b", 0x02) # Illegal Data Address
|
|
if debug:
|
|
print("Illegal Data Address: ")
|
|
print(err)
|
|
else:
|
|
RESPONSE += struct.pack("b", function_code)
|
|
RESPONSE += struct.pack(">h", register_address)
|
|
RESPONSE += struct.pack(">h", self.REGISTER[register_address])
|
|
elif function_code == 0x10:
|
|
starting_address = struct.unpack(">h", data[:2])[0]
|
|
quantity_of_registers = struct.unpack(">h", data[2:4])[0]
|
|
byte_of_registers = struct.unpack("b", data[4:5])[0]
|
|
try:
|
|
if byte_of_registers != 2 * quantity_of_registers:
|
|
raise struct.error
|
|
values = struct.unpack(">%dh" % quantity_of_registers, data[5:])
|
|
except struct.error as err:
|
|
RESPONSE += struct.pack("b", function_code | 0x80)
|
|
RESPONSE += struct.pack("b", 0x03) # Illegal Data Value
|
|
if debug:
|
|
print("Illegal Data Value, data length error")
|
|
else:
|
|
try:
|
|
for i in range(quantity_of_registers):
|
|
self.REGISTER[starting_address + i] = values[i]
|
|
except IndexError as err:
|
|
RESPONSE += struct.pack("b", function_code | 0x80)
|
|
RESPONSE += struct.pack("b", 0x02) # Illegal Data Address
|
|
if debug:
|
|
print("Illegal Data Address: ")
|
|
print(err)
|
|
else:
|
|
RESPONSE += struct.pack("b", function_code)
|
|
RESPONSE += struct.pack(">h", starting_address)
|
|
RESPONSE += struct.pack(">h", quantity_of_registers)
|
|
else:
|
|
RESPONSE += struct.pack("b", function_code | 0x80)
|
|
RESPONSE += struct.pack("b", 0x01) # Illegal Function
|
|
RESPONSE += self.crc16(RESPONSE)
|
|
if debug:
|
|
print("FUNCTION CODE: ", function_code)
|
|
print("RESPONSE: ", RESPONSE)
|
|
self.uart.write(RESPONSE)
|