53 lines
2.4 KiB
GDScript
53 lines
2.4 KiB
GDScript
class_name navigation extends Node
|
||
|
||
## "Протокол №120-11435-09.PDF"
|
||
|
||
const ONLINE_TIMEOUT: = 5000 ## Время ожидания пакета, миллисекунды
|
||
const pow_2_30: = pow(2.0, 30.0)
|
||
const FRACT_MAX: int = 3
|
||
|
||
## Источник данных навигации
|
||
class NaviSource extends unit.Unit:
|
||
var cmd_tick_rx: int = 0
|
||
var fract: int = 0
|
||
var data: = {} ## Принятые данные
|
||
var source_addr: = [] ## Адрес отправителя
|
||
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)
|
||
source_addr.clear()
|
||
|
||
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() < 32:
|
||
emit_signal('parse_failed', self)
|
||
return false
|
||
var flags: = rx_data.decode_u16(28) # Флаги достоверности полей структуры
|
||
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) # Курс, градусы (0.0 - Север)
|
||
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(24) #
|
||
if fract == 0:
|
||
emit_signal('data_received', data)
|
||
fract += 1
|
||
fract %= FRACT_MAX
|
||
return true
|