198 lines
9.0 KiB
GDScript
198 lines
9.0 KiB
GDScript
extends Node
|
||
|
||
const ONLINE_TIMEOUT = 5.0 # Время ожидания пакета от ячейки, секунды
|
||
const RESEND_TIMEOUT = 0.3 # Задержка для повторной отправки команды, секунды
|
||
const RETRY_COUNT = 5 # Количество повторных отправок
|
||
const DEFAULT_ADDR = ['localhost', 50000]
|
||
const BUFFER_SIZE = 2048
|
||
|
||
# Ячейка ЯУ-07
|
||
class YaU07:
|
||
|
||
enum CmdCode {
|
||
READ_ISA = 0, # Код команды "читать порт ISA" ячейки ЯУ-07
|
||
WRITE_ISA = 1 } # Код команды "записать порт ISA" ячейки ЯУ-07
|
||
|
||
enum CmdState {
|
||
UNCK, # Номер команды не ининцирован
|
||
SEND, # Буфер готов для отправки
|
||
WAIT, # Ожидание выполнения
|
||
DONE, # Команда выполнена
|
||
FAIL } # Команда не выполнена
|
||
|
||
signal online_changed(yau07: YaU07)
|
||
signal command_done(yau07: YaU07)
|
||
signal command_fail(yau07: YaU07)
|
||
signal net_error(message: String)
|
||
|
||
var src_addr: = DEFAULT_ADDR # Адрес источника
|
||
var cmd_num_rx: = 0 # Номер команды принятый
|
||
var cmd_num_tx: = 0 # Номер команды переданный
|
||
var cmd_state = CmdState.UNCK # Состояние выполнения команды
|
||
var cmd_result: = 1 # Результат выполнения команды. Устанавливается в ЯУ-07
|
||
var cmd_data: = PackedByteArray() # Данные команды. Устанавливается в ЯУ-07
|
||
var cmd_retry = 0
|
||
var tick = 0.0
|
||
var cmd_tick_tx = 0.0
|
||
var cmd_tick_rx = 0.0
|
||
var status: = PackedByteArray() # Поле "состояние прибора"
|
||
var tx_data: = [] # Буфер отправленный
|
||
var tx_len = 0
|
||
var isa_ports = {} # Результат выполнения команды "читать порт"
|
||
var online = false
|
||
|
||
func _to_string() -> String:
|
||
return '%s %s:%d cmd_num:%d' % [['отключен', 'на связи'][int(online)], src_addr[0], src_addr[1], cmd_num_rx]
|
||
|
||
|
||
func _init(addr: Array):
|
||
src_addr = addr
|
||
tx_data.resize(BUFFER_SIZE)
|
||
tx_data.fill(0)
|
||
|
||
|
||
func process(delta: float, sock: PacketPeerUDP):
|
||
tick += delta
|
||
if cmd_state == CmdState.SEND:
|
||
sock.set_dest_address(src_addr[0], src_addr[1])
|
||
if sock.put_packet(tx_data.slice(0, tx_len)) == OK:
|
||
cmd_tick_tx = tick
|
||
cmd_state = CmdState.WAIT
|
||
cmd_retry = RETRY_COUNT
|
||
elif cmd_state == CmdState.WAIT:
|
||
if cmd_retry:
|
||
if (tick - cmd_tick_tx) > RESEND_TIMEOUT:
|
||
sock.set_dest_address(src_addr[0], src_addr[1])
|
||
if sock.put_packet(tx_data.slice(0, tx_len)) == OK:
|
||
cmd_tick_tx = tick
|
||
else:
|
||
emit_signal('socket_error', 'ошибка: пакет не отправлен на: %s' % src_addr)
|
||
cmd_retry -= 1
|
||
else:
|
||
cmd_state = CmdState.FAIL
|
||
emit_signal('command_fail', self)
|
||
if online and ((tick - cmd_tick_rx) > ONLINE_TIMEOUT):
|
||
online = false
|
||
emit_signal('online_changed', self)
|
||
|
||
|
||
func send_isa(cmd_code: int, ports_data: Array) -> bool:
|
||
if (cmd_state == CmdState.WAIT) or (cmd_state == CmdState.SEND):
|
||
return false
|
||
if not ((cmd_code == CmdCode.READ_ISA) or (cmd_code == CmdCode.WRITE_ISA)):
|
||
return false
|
||
if cmd_state == CmdState.UNCK:
|
||
return false
|
||
|
||
cmd_num_tx = (cmd_num_rx + 1) % 0xffff
|
||
|
||
var ports_count = ports_data.size()
|
||
tx_len = ports_count * 2 + 8
|
||
Tools.set_wordi(tx_data, 2, cmd_num_tx)
|
||
|
||
if cmd_code == CmdCode.WRITE_ISA:
|
||
ports_count /= 2
|
||
tx_data[6] = cmd_code
|
||
tx_data[7] = ports_count
|
||
|
||
var i: = 0
|
||
for word in ports_data:
|
||
Tools.set_wordi(tx_data, 8 + i, word)
|
||
i += 2
|
||
cmd_state = CmdState.SEND
|
||
return true
|
||
|
||
|
||
func get_cmd_code(rx_data):
|
||
""" Возвращает код команды ЯУ-07
|
||
:param rx_data: Принятые данные
|
||
:return: None - Код команды не найден, 0...255 - код команды """
|
||
|
||
var rx_data_len = rx_data.size()
|
||
if rx_data_len < 7:
|
||
return null
|
||
var status_len = Tools.ibytes_to_word(rx_data, 6) # Длина массива байтового "состояние прибора"
|
||
if rx_data_len <= (status_len + 6): # Нет поля "ответ на команду", разбор пакета завершить
|
||
return null
|
||
var cmd_code_pos = status_len + 8 # Положение в приёмном буфере "кода команды"
|
||
if rx_data_len <= cmd_code_pos:
|
||
return null
|
||
return rx_data[cmd_code_pos] # "код команды"
|
||
|
||
|
||
func parse_isa(rx_data, status_len) -> bool:
|
||
var ports_count_pos = status_len + 10 # Положение в приёмном буфере количества портов
|
||
if rx_data.size() < ports_count_pos:
|
||
return false
|
||
var ports_count = rx_data[ports_count_pos] # Количество портов
|
||
var counted_len = status_len + (ports_count * 4) + 11 # Вычисленная длина пакета
|
||
if rx_data.size() < counted_len:
|
||
return false
|
||
var addr_pos = status_len + 11 # Смещение положения адреса в приёмном буфере
|
||
var data_pos = status_len + 13 # Смещение положения данных в приёмном буфере
|
||
var i: = 0
|
||
while i < ports_count:
|
||
var shift: = i * 4
|
||
var isa_addr = Tools.ibytes_to_word(rx_data, addr_pos + shift) # Адрес порта ISA яу-07
|
||
var isa_data = Tools.ibytes_to_word(rx_data, data_pos + shift) # Данные порта ISA яу-07
|
||
isa_ports[isa_addr] = isa_data
|
||
i += 1
|
||
cmd_data = rx_data.subarray(addr_pos, (addr_pos + ports_count * 4) - 1)
|
||
return i > 0
|
||
|
||
|
||
func parse(rx_data: PackedByteArray) -> bool:
|
||
"""
|
||
Заполняет состояние прибора данными прочитанными из портов ЯУ-07
|
||
для команд "читать порты" или "записать порты"
|
||
:param rx_data: Принятые данные
|
||
:return: True - Данные обработаны успешно,
|
||
False - Данные не обработаны из за ошибки """
|
||
|
||
if not online:
|
||
online = true
|
||
emit_signal('online_changed', self)
|
||
|
||
var rx_data_len = rx_data.size()
|
||
if rx_data_len < 11:
|
||
return false
|
||
|
||
cmd_num_rx = Tools.ibytes_to_word(rx_data, 2)
|
||
if cmd_state == CmdState.UNCK:
|
||
cmd_state = CmdState.DONE
|
||
|
||
var status_len = Tools.ibytes_to_word(rx_data, 6) # Длина массива байтового "состояние прибора"
|
||
if rx_data_len < (status_len + 6):
|
||
return false
|
||
var i: = 0
|
||
var len_rx_data = rx_data.size()
|
||
status.resize(status_len)
|
||
while i < status_len:
|
||
var byte_i = i + 8
|
||
if byte_i < len_rx_data:
|
||
status[i] = rx_data[byte_i] # Массив байтовый "состояние прибора"
|
||
i += 1
|
||
|
||
if rx_data_len == (status_len + 6):
|
||
return true # Нет поля "ответ на команду", разбор пакета завершить
|
||
|
||
var cmd_code_pos = status_len + 8 # Положение в приёмном буфере "код команды"
|
||
if rx_data_len <= cmd_code_pos:
|
||
return false
|
||
|
||
var cmd_status_pos = status_len + 9 # Положение в приёмном буфере "состояния выполнения команды"
|
||
if rx_data_len <= cmd_status_pos:
|
||
return false
|
||
cmd_result = rx_data[cmd_status_pos]
|
||
if cmd_result != 1: # Команда не выполнена
|
||
return false
|
||
|
||
var cmd_code: CmdCode = rx_data[cmd_code_pos] # "код команды"
|
||
if (cmd_code == CmdCode.READ_ISA) or (cmd_code == CmdCode.WRITE_ISA):
|
||
parse_isa(rx_data, status_len)
|
||
|
||
if (cmd_state == CmdState.WAIT) and (cmd_num_rx == cmd_num_tx):
|
||
cmd_state = CmdState.DONE
|
||
emit_signal('command_done', self)
|
||
return true
|