class_name tcp5_p28 extends Node ## Реализация "Протокол информационного сопряжения. ## СПО АСУ изделия МП-550 с СПО 5П-28 const BUFFER_SIZE = 2048 ## Размер приёмного буфера для датаграмм. ## Тип модуляции для запаковки в пакет для 5П-28. const MOD_TYPES: Array = [ '', 'am', 'ook', 'ask', 'fm', 'qpsk', 'oqpsk', 'pi4qpsk', 'qam16', 'qam32', 'qam64', 'qam128', 'qam256', 'qam512', 'cpfsk', 'psk', 'ofdm' ] const CU_STATE: Dictionary = { 0: 2, # Подготовка к выполнению 1: 1, # Выполняется 2: 3, # Ошибка в команде 3: 7, # Занят выполнением другого ЦУ 4: 2, # Неисправность } class TCP5P28 extends unit.Unit: const PACK_TYPE_CMD: int = 1 const PACK_TYPE_RES: int = 2 const CMD_TYPE_STATE: int = 1 const CMD_TYPE_TH: int = 2 const CMD_TYPE_IN: int = 3 const PR_STATE: Dictionary = { 'уарэп-эмс': [1, 1], 'уарэп-яу07-1н': [9, 8], 'уарэп-яу07-1в': [5, 4], 'уарэп-яу07-1к': [13, 12], 'уарэп-яу07-2н': [10, 9], 'уарэп-яу07-2в': [6, 5], 'уарэп-яу07-2к': [14, 13], 'уарэп-яу07-3н': [11, 10], 'уарэп-яу07-3в': [7, 6], 'уарэп-яу07-3к': [15, 14], 'уарэп-яу07-4н': [12, 11], 'уарэп-яу07-4в': [8, 7], 'уарэп-яу07-4к': [16, 15] } signal data_sended() signal disconnected(rc: Array) signal connected(rc: Array) signal get_threats(unit: TCP5P28) ## Сигнал запрос целей. signal get_interfers(unit: TCP5P28) ## Запрос состояния выполнения целеуказания. var uarep_state: int = 1 ## Флаги состояния прибора, 0 бит, состояние ПО var uarep_line: int = 3 ## Состояние линии связи с прибором var ext_cu: bool = false var tx_queue: Array ## Очередь пакетов для отправки var tx_mutex: Mutex ## Мютекс для очереди передачи var thread: Thread ## Рабочий поток var tctl_mutex: Mutex ## Мютекс для управления потоком var tctl_run: bool ## Управление потоком enum STREAM_STATE { IDLE, CONNECT, WAIT, RX_START, RX_DONE, TX_START, TX_DONE, ERROR } func _init(nm) -> void: super._init(nm) ext_cu = ProjectSettings.get_setting('application/config/Внешнее управление', false) ProjectSettings.connect('settings_changed', on_setting_changed) tx_mutex = Mutex.new() tctl_mutex = Mutex.new() connect('data_received', on_data_received) connect('data_sended', on_data_sended) connect('connected', on_connected) connect('disconnected', on_disconnected) init_state() func on_connected(rc: Array): log.info('Клиент 5П-28 подключен: %s:%s' % [ rc[0], rc[1]] ) func on_data_sended(): pass func on_data_received(data: PackedByteArray): var tick: = Time.get_ticks_msec() parse(data, tick) func on_disconnected(rc: Array): log.info('Клиент 5П-28 отключен: %s:%s' % [ rc[0], rc[1]] ) if online: online = false emit_signal('line_changed', self) tx_mutex.lock() tx_queue.clear() tx_mutex.unlock() func init_state(): for key in PR_STATE.keys(): var unit_pribor = network.get_unit_instance(key) unit_pribor.connect('line_changed', Callable(self, 'pribor_line_changed').bind(key)) func open(host: String, port: int): tctl_run = true thread = Thread.new() thread.start(thread_proc.bind(host, port)) func close(): tctl_mutex.lock() tctl_run = false tctl_mutex.unlock() func thread_proc(host: String, port: int): var sock = TCPServer.new() var stream: = StreamPeerTCP.new() var fsm: = STREAM_STATE.CONNECT var rx_buff: PackedByteArray var rx_bytes: int = 0 var head_len: int = 8 ## Длина заголовка команды. var len_place: int = 4 ## Номер начального байта с длинной блока данных. var status: = StreamPeerTCP.Status.STATUS_NONE var rc: = sock.listen(port, host) var cl_host: String var cl_port: int while true: stream.poll() var new_status = stream.get_status() if status != new_status: status = new_status match status: stream.STATUS_NONE: call_deferred('emit_signal', 'disconnected', [cl_host, cl_port]) fsm = STREAM_STATE.ERROR stream.STATUS_CONNECTED: cl_host = stream.get_connected_host() cl_port = stream.get_connected_port() call_deferred('emit_signal', 'connected', [cl_host, cl_port]) stream.STATUS_ERROR: fsm = STREAM_STATE.ERROR if fsm == STREAM_STATE.CONNECT: if sock.is_connection_available(): # Проверить, если кто то пытается подключиться stream = sock.take_connection() # Принять соединение new_status = stream.get_status() if new_status == stream.STATUS_CONNECTED: fsm = STREAM_STATE.WAIT else: fsm = STREAM_STATE.ERROR elif fsm == STREAM_STATE.WAIT: if status == stream.STATUS_CONNECTED: fsm = STREAM_STATE.RX_START else: OS.delay_msec(50) fsm = STREAM_STATE.CONNECT elif fsm == STREAM_STATE.ERROR: stream.disconnect_from_host() fsm = STREAM_STATE.WAIT OS.delay_msec(1000) elif fsm == STREAM_STATE.IDLE: OS.delay_msec(50) fsm = STREAM_STATE.RX_START elif fsm == STREAM_STATE.RX_START: rx_buff.clear() rx_bytes = 0 var sz = stream.get_available_bytes() if sz <= 0: fsm = STREAM_STATE.TX_START continue var rxd = stream.get_data(head_len) if rxd[0] == Error.OK: rx_buff.append_array(rxd[1]) else: fsm = STREAM_STATE.ERROR continue var pay: = rx_buff.decode_u32(len_place) rx_bytes = pay + head_len if rx_buff.size() == rx_bytes: fsm = STREAM_STATE.RX_DONE else: rxd = stream.get_data(rx_bytes - rx_buff.size()) rc = rxd[0] if rc == Error.OK: rx_buff.append_array(rxd[1]) fsm = STREAM_STATE.RX_DONE else: fsm = STREAM_STATE.ERROR elif fsm == STREAM_STATE.RX_DONE: var rx_data: = rx_buff.duplicate() call_deferred('emit_signal', 'data_received', rx_data) fsm = STREAM_STATE.TX_START elif fsm == STREAM_STATE.TX_START: tx_mutex.lock() if not tx_queue.size(): fsm = STREAM_STATE.IDLE tx_mutex.unlock() continue var data = tx_queue.pop_front() tx_mutex.unlock() rc = stream.put_data(data) if rc == Error.OK: fsm = STREAM_STATE.TX_DONE else: fsm = STREAM_STATE.ERROR elif fsm == STREAM_STATE.TX_DONE: call_deferred('emit_signal', 'data_sended') fsm = STREAM_STATE.RX_START func pribor_line_changed(u, key): uarep_state = tools.set_bit(uarep_state, PR_STATE[key][0], u.online) uarep_line = tools.set_bit(uarep_line, PR_STATE[key][1], u.online) func _to_string() -> String: return String('5П-28: "%s" %s ' % [self.name, ['отключен', 'на связи'][int(online)]]) func parse(data: PackedByteArray, tick: int): rx_tick = tick if not online: online = true emit_signal('line_changed', self) var pack_type = data.decode_u8(2) if pack_type == PACK_TYPE_CMD: var cmd_type = data.decode_u8(8) if cmd_type == CMD_TYPE_TH: emit_signal('get_threats') if cmd_type == CMD_TYPE_STATE: emit_signal('get_interfers') if cmd_type == CMD_TYPE_IN and ext_cu: set_interfer(data) func queue_packet(packet: PackedByteArray): tx_mutex.lock() tx_queue.append(packet) tx_mutex.unlock() func pack_threats(ths: Dictionary): var data = PackedByteArray() var data_len = 4 + len(ths) * 80 # длинна блока данных в байтах var tick0 = Time.get_ticks_msec() data.resize(4) data.encode_u16(0, 0) # Код ошибки data.encode_u16(2, len(ths)) # Количество целей for th in ths.values(): data.append_array(get_threats_data(th, tick0)) var data_to_send: PackedByteArray = create_header(data_len) data_to_send.append_array(data) queue_packet(data_to_send) func pack_interfers(interfers: Dictionary, ecms: Dictionary): var data = PackedByteArray() var data_len = 12 # длинна блока данных в байтах data.resize(data_len) data.encode_u16(0, 0) # Код ошибки data.encode_u16(2, interfers.size()) # Количество целей data.encode_u32(4, uarep_state) # Состояние УА-РЕП data.encode_u32(8, uarep_line) # Состояние линий связи var cu_data: PackedByteArray = [] cu_data.resize(4) for id in interfers.values(): if not ecms.has(id): push_error('не обнаружен ключ: %s, в словаре воздействий' %id) continue var cu = ecms[id] cu_data.encode_u16(0, cu.ispp) var state: int = CU_STATE[cu.svk] if CU_STATE.has(cu.svk) else 8 cu_data.encode_u8(2, state) # Состояние выполнения команды cu_data.encode_u8(3, 0xFF) # Эффективность работы помехи data.append_array(cu_data) var data_to_send: PackedByteArray = create_header(data_len + 4 * interfers.size()) data_to_send.append_array(data) queue_packet(data_to_send) func create_header(data_len: int): var head_data = PackedByteArray() head_data.resize(8) # Размер заголовка head_data.encode_u16(0, 0) # Код ошибки head_data.encode_u8(2, 2) # Тип пакета, ответ на запрос head_data.encode_u8(3, 5) # Версия протокола, всегда 5 head_data.encode_u32(4, data_len) # Длинна пакета данных return head_data func get_threats_data(th: threats.Threat, tick0: int): var th_data = PackedByteArray() th_data.resize(80) # Количество байт в массиве th_data.encode_u64(0, tick0) var flags: int = 0 # с 0 по 7 бит рекомендованная помеха flags |= 0 << 8 # Флаг рекомендуемой помехи flags |= int(th.fflags['aoa']) << 9 # Флаг доставерности пеленга flags |= int(th.fflags['alt']) << 10 # Флаг доставерности высоты flags |= int(th.fflags['pow']) << 11 # Флаг доставерности мощности flags |= int(th.fflags['baud']) << 12 # Флаг доставерности ск. мод. flags |= int(th.fflags['slon']) << 13 # Флаг доставерности долготы ст. позиции flags |= int(th.fflags['slat']) << 14 # Флаг доставерности широты ст. позиции flags |= int(th.fflags['lon']) << 15 # Флаг доставерности долготы flags |= int(th.fflags['lat']) << 16 # Флаг доставерности широты flags |= int(0) << 30 # Флаг признак назначения помехи на цель flags |= int(0) << 31 # Флаг признак назначения опасной цель th_data.encode_u32(8, flags) # Флаги th_data.encode_u16(12, th.id) # Идентификатор объекта th_data.encode_u16(14, int(round(th.aoa/0.1))) # Пеленг объекта th_data.encode_u16(16, int(th.alt)) # Высота объекта th_data.encode_u16(18, int(th.freq)) # Частота объекта th_data.encode_u16(20, int(th.width)) # Ширина частотной полосы объекта th_data.encode_u8(22, int(th.pow)) # Мощность излучения объекта var mod = MOD_TYPES.find(th.tmod) if mod == -1: mod = 0 th_data.encode_u8(23, int(mod)) # Тип модуляции th_data.encode_u32(24, int(th.baud)) # Скорость модуляции, бод to_cp866(th.proto, th_data) var c1 = pow(2, 30) / 180.0 th_data.encode_s32(64, int(th.slon * c1)) th_data.encode_s32(68, int(th.slat * c1)) th_data.encode_s32(72, int(th.lon * c1)) th_data.encode_s32(76, int(th.lat * c1)) return th_data func to_cp866(proto: String, th_data: PackedByteArray): var cp866_proto: PackedByteArray = proto.to_ascii_buffer() cp866_proto.resize(36) for i in cp866_proto.size(): th_data[28 + i] = cp866_proto[i] func set_interfer(data: PackedByteArray): var mode = data.decode_u8(9) var id = data.decode_u16(10) var aoa = float(data.decode_u16(12)) / 10 var ela = float(data.decode_u16(14)) / 10 var freq = data.decode_u16(16) var width = data.decode_u16(18) var interfer_params: Array = [mode, id, aoa, ela, freq, width] signaller.emit_signal('interfer_rcv', interfer_params) func on_interfer_accept(err: bool): var data_tx = PackedByteArray() var data_len = 2 # длинна блока данных в байтах data_tx.resize(data_len) data_tx.encode_u16(0, err) # Код ошибки var data_to_send: PackedByteArray = create_header(data_len) data_to_send.append_array(data_tx) queue_packet(data_to_send) func on_setting_changed(): ext_cu = ProjectSettings.get_setting('application/config/Внешнее управление', false)