359 lines
16 KiB
GDScript
359 lines
16 KiB
GDScript
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)
|