47 lines
1.9 KiB
GDScript
47 lines
1.9 KiB
GDScript
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
|