mirror of
https://github.com/openmv/openmv.git
synced 2025-11-04 14:49:50 +08:00
Update modbus.py
add more check
This commit is contained in:
parent
8c8e1abbd8
commit
f38dcc3505
@ -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)
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user