Update modbus.py

add more check
This commit is contained in:
王开智 2019-11-04 19:32:41 +08:00 committed by GitHub
parent 8c8e1abbd8
commit f38dcc3505
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23

View File

@ -1,15 +1,12 @@
import struct import struct
class ModbusRTU(): class ModbusRTU():
def __init__(self, uart, slave_id=0x01): def __init__(self, uart, slave_id=0x01, register_num=30):
self.SLAVE_ID = slave_id self.SLAVE_ID = slave_id
self.uart = uart self.uart = uart
self.uart.init(19200, bits=8, parity=0, stop=1, timeout=1, timeout_char=4) self.register_num = register_num
self.HOLDING_REGISTERS = [0]*30 self.REGISTER = [0]*self.register_num
def any(self): self.CRC16_TABLE = [
return self.uart.any()
def crc16(self, data):
crc = 0xFFFF
CRC16_TABLE = [
0x0000,0xC0C1,0xC181,0x0140,0xC301,0x03C0,0x0280,0xC241,0xC601, 0x0000,0xC0C1,0xC181,0x0140,0xC301,0x03C0,0x0280,0xC241,0xC601,
0x06C0,0x0780,0xC741,0x0500,0xC5C1,0xC481,0x0440,0xCC01,0x0CC0, 0x06C0,0x0780,0xC741,0x0500,0xC5C1,0xC481,0x0440,0xCC01,0x0CC0,
0x0D80,0xCD41,0x0F00,0xCFC1,0xCE81,0x0E40,0x0A00,0xCAC1,0xCB81, 0x0D80,0xCD41,0x0F00,0xCFC1,0xCE81,0x0E40,0x0A00,0xCAC1,0xCB81,
@ -38,28 +35,48 @@ class ModbusRTU():
0x48C0,0x4980,0x8941,0x4B00,0x8BC1,0x8A81,0x4A40,0x4E00,0x8EC1, 0x48C0,0x4980,0x8941,0x4B00,0x8BC1,0x8A81,0x4A40,0x4E00,0x8EC1,
0x8F81,0x4F40,0x8D01,0x4DC0,0x4C80,0x8C41,0x4400,0x84C1,0x8581, 0x8F81,0x4F40,0x8D01,0x4DC0,0x4C80,0x8C41,0x4400,0x84C1,0x8581,
0x4540,0x8701,0x47C0,0x4680,0x8641,0x8201,0x42C0,0x4380,0x8341, 0x4540,0x8701,0x47C0,0x4680,0x8641,0x8201,0x42C0,0x4380,0x8341,
0x4100,0x81C1,0x8081,0x4040 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: for char in data:
crc = (crc >> 8) ^ CRC16_TABLE[((crc) ^ char) & 0xFF] crc = (crc >> 8) ^ self.CRC16_TABLE[((crc) ^ char) & 0xFF]
return struct.pack('<H',crc) return struct.pack('<H',crc)
def handle(self): def handle(self, debug = False):
REQUEST = self.uart.read() REQUEST = self.uart.read()
if debug:
print("GOT REQUEST: ", REQUEST)
additional_address = REQUEST[0] additional_address = REQUEST[0]
error_check = REQUEST[-2:] error_check = REQUEST[-2:]
function_code = REQUEST[1] function_code = REQUEST[1]
data = REQUEST[2:] data = REQUEST[2:]
RESPONSE = struct.pack('b', self.SLAVE_ID) 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 self.SLAVE_ID != additional_address:
RESPONSE += struct.pack('b', function_code|0x80) if debug:
RESPONSE += struct.pack('b', 0x04)#SLAVE_ID print("OpenMV slave id: ", self.SLAVE_ID)
elif self.crc16(REQUEST[:-2]) != error_check: print("got cmd id: ", additional_address)
RESPONSE += struct.pack('b', function_code|0x80) return 0 # do nothing
RESPONSE += struct.pack('b', 0x04)#CRC error if function_code == 0x03:
elif function_code == 0x03:
starting_address = struct.unpack('>h', data[:2])[0] starting_address = struct.unpack('>h', data[:2])[0]
quantity_of_registers = struct.unpack('>h', data[2:4])[0] quantity_of_registers = struct.unpack('>h', data[2:4])[0]
response_registers = self.HOLDING_REGISTERS[starting_address:starting_address+quantity_of_registers] 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', function_code)
RESPONSE += struct.pack('b', 2*quantity_of_registers) RESPONSE += struct.pack('b', 2*quantity_of_registers)
for b in response_registers: for b in response_registers:
@ -67,26 +84,50 @@ class ModbusRTU():
elif function_code == 0x06: elif function_code == 0x06:
register_address = struct.unpack('>h', data[:2])[0] register_address = struct.unpack('>h', data[:2])[0]
register_value = struct.unpack('>h', data[2:4])[0] register_value = struct.unpack('>h', data[2:4])[0]
self.HOLDING_REGISTERS[register_address] = register_value 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('b', function_code)
RESPONSE += struct.pack('>h', register_address) RESPONSE += struct.pack('>h', register_address)
RESPONSE += struct.pack('>h', self.HOLDING_REGISTERS[register_address]) RESPONSE += struct.pack('>h', self.REGISTER[register_address])
elif function_code == 0x10: elif function_code == 0x10:
starting_address = struct.unpack('>h', data[:2])[0] starting_address = struct.unpack('>h', data[:2])[0]
quantity_of_registers = struct.unpack('>h', data[2:4])[0] quantity_of_registers = struct.unpack('>h', data[2:4])[0]
byte_of_registers = struct.unpack('b', data[4:5])[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:]) values = struct.unpack('>%dh'%quantity_of_registers, data[5:])
self.HOLDING_REGISTERS[starting_address:starting_address+quantity_of_registers] = values except struct.error as err:
if byte_of_registers == 2*quantity_of_registers: 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('b', function_code)
RESPONSE += struct.pack('>h', starting_address) RESPONSE += struct.pack('>h', starting_address)
RESPONSE += struct.pack('>h', quantity_of_registers) RESPONSE += struct.pack('>h', quantity_of_registers)
else: else:
RESPONSE += struct.pack('b', function_code|0x80) RESPONSE += struct.pack('b', function_code|0x80)
RESPONSE += struct.pack('b', 0x04) RESPONSE += struct.pack('b', 0x01) # Illegal Function
else:
RESPONSE += struct.pack('b', function_code|0x80)
RESPONSE += struct.pack('b', 0x01)#not support
RESPONSE += self.crc16(RESPONSE) RESPONSE += self.crc16(RESPONSE)
print(RESPONSE) if debug:
print("FUNCTION CODE: ", function_code)
print("RESPONSE: ", RESPONSE)
self.uart.write(RESPONSE) self.uart.write(RESPONSE)