diff --git a/project.godot b/project.godot index 821da95d..ce0e5d20 100644 --- a/project.godot +++ b/project.godot @@ -64,6 +64,7 @@ sch3="*res://scripts/sch3.gd" spt25="*res://scripts/spt25.gd" astrakb="*res://scripts/astra-kb.gd" prd="*res://scripts/prd.gd" +navi="*res://scripts/navigation.gd" [debug] diff --git a/scripts/navigation.gd b/scripts/navigation.gd new file mode 100644 index 00000000..3e9a198b --- /dev/null +++ b/scripts/navigation.gd @@ -0,0 +1,46 @@ +class_name navigation extends Node + +## "Протокол №120-11435-09.PDF" + +const ONLINE_TIMEOUT: = 5000 ## Время ожидания пакета от ячейки, милисекунды +const BUFFER_SIZE: = 32 ## Размер буфера для приёма датаграмм +const pow_2_30: = pow(2.0, 30.0) + +## Источник данных навигации +class NaviSource extends unit.Unit: + var cmd_tick_rx: int = 0 + var data: = {} + const akng_mode = {0:'тестовый', 1:'основной', 2:'резервный', 3:'аварийный'} + func _to_string() -> String: + return String('NaviSource: %s %s' % [self.name, 'на связи' if online else 'отключен']) + func _init(nm): + self.name = nm + func process(tick: int): + if online and ((tick - cmd_tick_rx) > ONLINE_TIMEOUT): + online = false + emit_signal('line_changed', self) + + func parse(rx_data: PackedByteArray, tick: int) -> bool: + cmd_tick_rx = tick + if not online: + online = true + emit_signal('line_changed', self) + if rx_data.size() < 28: + emit_signal('parse_failed', self) + return false + var flags: = rx_data.decode_u16(24) + data.clear() + data['mode'] = akng_mode[(flags >> 4) & 3]; + if flags & (1 << 0): + data['kk'] = rx_data.decode_float(0) + data['bk'] = rx_data.decode_float(4) + if flags & (1 << 1): + data['k'] = rx_data.decode_float(8) + if flags & (1 << 2): + data['latitude'] = float(rx_data.decode_s32(12) * 180) / pow_2_30 + data['longitude'] = float(rx_data.decode_s32(16) * 180) / pow_2_30 + if flags & (1 << 3): + data['speed'] = rx_data.decode_float(20) + data['err'] = rx_data.decode_u16(22) + emit_signal('data_received', data) + return true diff --git a/scripts/navigation.gd.uid b/scripts/navigation.gd.uid new file mode 100644 index 00000000..a980a8b9 --- /dev/null +++ b/scripts/navigation.gd.uid @@ -0,0 +1 @@ +uid://bsabhln1eblsg diff --git a/scripts/network.gd b/scripts/network.gd index a1df43e7..acc8a43d 100644 --- a/scripts/network.gd +++ b/scripts/network.gd @@ -1,7 +1,7 @@ class_name Network extends Node -const PROTOCOL_TYPES: = {'yau07tx': yau07.YaU07, 'json-capsrpb': capsrpb.CapsRpb} +const PROTOCOL_TYPES: = {'yau07tx': yau07.YaU07, 'json-capsrpb': capsrpb.CapsRpb, 'udprawrx': navi.NaviSource} const TCP_PROTO: = {'5p28': tcp5p28.TCP5P28} const SERIAL_PROTO: = {'uart': spt25.SPT25} const MODBUS_PROTO: = {'mbcs': sch3.Sch3} @@ -77,7 +77,7 @@ func create_socket(nm) -> Socket: sock.set_broadcast_enabled(broad) var rc = Error.OK if not bind else sock.bind(port, addr) var errlevel = log.INFO if rc == Error.OK else log.ERROR - log.message(errlevel, '%s: сокет:%s:%d привязанный:%s %s' % [nm, addr, port, ['нет', 'да'][int(bind)], error_string(rc)]) + log.message(errlevel, '\"%s\" привязан к %s:%d? - %s, результат: %s' % [nm, addr, port, ['нет', 'да'][int(bind)], error_string(rc)]) return sock @@ -110,6 +110,7 @@ func _ready() -> void: poll_sockets.append(create_socket('уарэп-яу07-частный')) poll_sockets.append(create_socket('уарэп-яу07-общий')) poll_sockets.append(create_socket('уарэп-капсрпб')) + poll_sockets.append(create_socket('навигация')) sock_unicast = poll_sockets[0] sock_capsrpb = poll_sockets[2] diff --git a/scripts/settings.gd b/scripts/settings.gd index afb60102..e2dddd3d 100644 --- a/scripts/settings.gd +++ b/scripts/settings.gd @@ -25,6 +25,7 @@ var settings_program: Dictionary 'уарэп-яу07-3в': ['yau07tx', '10.1.1.32', [50032], false, false, 'IP-адрес ЯУ-07 прибора ПРД-В'], 'уарэп-яу07-3н': ['yau07tx', '10.1.1.31', [50031], false, false, 'IP-адрес ЯУ-07 прибора ПРД-Н'], 'уарэп-яу07-3к': ['yau07tx', '10.1.1.33', [50033], false, false, 'IP-адрес ЯУ-07 прибора ПРД-Н'], + 'навигация': ['udprawrx','*', [60000], true, true, 'IP-адрес на котором принимают сообщения от навигации'], # Название: [ Протокол, Адрес, Порт получения, Комментарий ] 'уарэп-5п28': [ '5p28', '127.0.0.1', [50000], 'IP-адрес комплекса 5П-28'], diff --git a/scripts/unit.gd b/scripts/unit.gd index 2e69501c..f97be624 100644 --- a/scripts/unit.gd +++ b/scripts/unit.gd @@ -16,7 +16,7 @@ class Unit extends Object: signal command_done(unit) ## Вызывается после успешного выполнения команды. signal command_fail(unit) ## Вызывается если возникла ошибка при выполнении команды и команда не выполнена. signal parse_failed(unit) ## Вызывается при обнаружении ошибки во входных данных - signal data_received(data: PackedByteArray) ## Вызывается по приёму данных + signal data_received(data) ## Вызывается по приёму данных var cmd_state = CmdState.UNCK ## Состояние выполнения команды var tx_data: = PackedByteArray() ## Буфер для отправки.