413 lines
16 KiB
GDScript
413 lines
16 KiB
GDScript
# server_node.gd
|
|
class_name ServerNode
|
|
extends Node
|
|
|
|
signal camera_position_updated(pos_x, pos_y, pos_z)
|
|
signal camera_rotation_updated(rot_x, rot_y, rot_z)
|
|
signal sensor_point_generated(pos_x, pos_y, pos_z)
|
|
|
|
var server = UDPServer.new()
|
|
var port = 5001
|
|
var awaiting_sensors = {}
|
|
var invalid_sensors = {}
|
|
var valid_sensors = {}
|
|
|
|
var use_thread: bool = false
|
|
var thread: Thread
|
|
var mutex: Mutex
|
|
var exit_thread = true
|
|
|
|
var signal_queue = []
|
|
|
|
func _ready():
|
|
ServerSignals.sensor_approved.connect(_on_sensor_approved)
|
|
ServerSignals.sensor_declined.connect(_on_sensor_declined)
|
|
server.listen(port, "0.0.0.0")
|
|
if not server.is_listening():
|
|
print('Unable to listen on port: ', port)
|
|
print('Listening on port: ', port)
|
|
|
|
mutex = Mutex.new()
|
|
if use_thread:
|
|
thread = Thread.new()
|
|
print('Starting UDP Server thread')
|
|
thread.start(_thread_function)
|
|
else:
|
|
print('UDP server running in single threaded mode')
|
|
|
|
func _process(delta: float) -> void:
|
|
if not use_thread:
|
|
_process_connections()
|
|
while signal_queue.size() > 0:
|
|
mutex.lock()
|
|
var signal_data = signal_queue.pop_front()
|
|
mutex.unlock()
|
|
if signal_data[0] == "new_sensor_connected":
|
|
SensorSignals.new_sensor_connected.emit(signal_data[1], signal_data[2])
|
|
elif signal_data[0] == "sensor_request_received":
|
|
ServerSignals.sensor_request_received.emit(signal_data[1], signal_data[2])
|
|
elif signal_data[0] == "on_time_data_received":
|
|
TimeHelpers.on_time_data_received(signal_data[1])
|
|
elif signal_data[0] == "gps_data_received":
|
|
DataSignals.gps_data_received.emit(signal_data[1], signal_data[2], signal_data[3], signal_data[4], signal_data[5])
|
|
elif signal_data[0] == "sat_data_received":
|
|
DataSignals.sat_data_received.emit(signal_data[1], signal_data[2], signal_data[3], signal_data[4])
|
|
elif signal_data[0] == "accelerometer_data_received":
|
|
DataSignals.accelerometer_data_received.emit(signal_data[1], signal_data[2], signal_data[3], signal_data[4], signal_data[5])
|
|
elif signal_data[0] == "magnetometer_data_received":
|
|
DataSignals.magnetometer_data_received.emit(signal_data[1], signal_data[2], signal_data[3])
|
|
elif signal_data[0] == "lidar_2d_data_received":
|
|
DataSignals.lidar_2d_data_received.emit(signal_data[1], signal_data[2], signal_data[3], signal_data[4])
|
|
|
|
func _exit_tree() -> void:
|
|
if use_thread:
|
|
mutex.lock()
|
|
exit_thread = true
|
|
mutex.unlock()
|
|
thread.wait_to_finish()
|
|
|
|
func _on_sensor_approved(sensor_ip: String):
|
|
mutex.lock()
|
|
valid_sensors[sensor_ip] = awaiting_sensors[sensor_ip]
|
|
if sensor_ip in awaiting_sensors:
|
|
awaiting_sensors.erase(sensor_ip)
|
|
signal_queue.append(["new_sensor_connected", sensor_ip, valid_sensors[sensor_ip]])
|
|
mutex.unlock()
|
|
|
|
func _on_sensor_declined(sensor_ip: String):
|
|
mutex.lock()
|
|
invalid_sensors[sensor_ip] = 0
|
|
if sensor_ip in awaiting_sensors:
|
|
awaiting_sensors.erase(sensor_ip)
|
|
mutex.unlock()
|
|
|
|
func _thread_function():
|
|
print('UDP Server thread started')
|
|
while true:
|
|
mutex.lock()
|
|
var should_exit = exit_thread
|
|
mutex.unlock()
|
|
if should_exit:
|
|
break
|
|
_process_connections()
|
|
|
|
func _process_connections():
|
|
server.poll()
|
|
if server.is_connection_available():
|
|
var peer = server.take_connection()
|
|
var packet = peer.get_packet()
|
|
if not _verify_checksum(packet):
|
|
print('Received packet with incorrect checksum: %s' % peer.get_packet_ip())
|
|
else:
|
|
|
|
_handle_new_connection(peer, packet)
|
|
|
|
func _handle_new_connection(peer: PacketPeerUDP, packet: PackedByteArray):
|
|
if packet.get(0) == 0xD4 and packet.get(1) == 0x53:
|
|
"""
|
|
| 2 bytes | 2 bytes | 2 bytes | variable length | 2 bytes |
|
|
| packet start | message class | payload length | payload | checksum |
|
|
First two bytes are the frame start - always 0xD4 and 0x53
|
|
The two bytes for message class and id identify the packet contents
|
|
The two bytes for payload length should be read as an u16 integer
|
|
and indicate the payload size.
|
|
The 2-byte checksuim is calculated over the packet's contents - the message
|
|
class and id bytes, payload length byts, and the payload itself.
|
|
The formula is:
|
|
var ck_a = 0
|
|
var ck_b = 0
|
|
for i in range(2, packet.size()-2):
|
|
ck_a += packet.get(i)
|
|
ck_b += ck_a
|
|
packet[packet.size()-2] = ck_a
|
|
packet[packet.size()-1] = ck_b
|
|
"""
|
|
_handle_six_fps_message(peer, packet)
|
|
else:
|
|
print("Received: '%s' %s:%s" % [packet.get_string_from_utf8(), peer.get_packet_ip(), peer.get_packet_port()])
|
|
|
|
func _calculate_checksum(packet) -> Array:
|
|
var ck_a = 0
|
|
var ck_b = 0
|
|
for i in range(2, packet.size()-2):
|
|
ck_a += packet.get(i)
|
|
ck_b += ck_a
|
|
return [ck_a % 0x100, ck_b % 0x100]
|
|
|
|
func _verify_checksum(packet) -> bool:
|
|
var calculated_checksum = _calculate_checksum(packet)
|
|
return packet.get(packet.size()-1) == calculated_checksum[1] and packet.get(packet.size()-2) == calculated_checksum[0]
|
|
|
|
func _handle_six_fps_message(peer: PacketPeerUDP, packet: PackedByteArray):
|
|
var peer_ip = peer.get_packet_ip()
|
|
if peer_ip in invalid_sensors:
|
|
return
|
|
if peer_ip in awaiting_sensors:
|
|
return
|
|
mutex.lock()
|
|
var valid_sensor: bool = peer.get_packet_ip() in valid_sensors
|
|
mutex.unlock()
|
|
var message_class = packet.get(2)
|
|
var message_id = packet.get(3)
|
|
if (not valid_sensor) and (message_class == 0x6e and message_id == 0x77):
|
|
_handle_new_peer_request(peer_ip, packet)
|
|
if not valid_sensor:
|
|
return
|
|
elif message_class == 0x6e and message_id == 0x4e:
|
|
_handle_heartbeat_packet(peer_ip, packet)
|
|
elif message_class == 0x2a and message_id == 0xac:
|
|
_handle_time_packet(peer_ip, packet)
|
|
elif message_class == 0x2a and message_id == 0x7c:
|
|
#print('GPS packet received')
|
|
_handle_position_packet(peer_ip, packet)
|
|
elif message_class == 0x2a and message_id == 0x7b:
|
|
#print('Sats packet received')
|
|
_handle_gps_sats_packet(peer_ip, packet)
|
|
elif message_class == 0x2a and message_id == 0xfa:
|
|
#print('Accel packet received')
|
|
_handle_accel_packet(peer_ip, packet)
|
|
elif message_class == 0x2a and message_id == 0x39:
|
|
#print('Mag packet received')
|
|
_handle_magnetic_packet(peer_ip, packet)
|
|
elif message_class == 0x2a and message_id == 0x6f:
|
|
_handle_lidar_2d_packet(peer_ip, packet)
|
|
elif message_class == 0x2a and message_id == 0xa7:
|
|
_handle_public_key_packet(peer_ip, packet)
|
|
elif message_class == 0x2a and message_id == 0x5e: # not in use
|
|
_handle_euler_packet(peer_ip, packet)
|
|
elif message_class == 0x2a and message_id == 0xba: # not in use
|
|
_handle_quaternion_packet(peer_ip, packet)
|
|
|
|
func _handle_new_peer_request(peer_ip: String, packet: PackedByteArray):
|
|
"""
|
|
offset | size | type | description
|
|
4 | 1 byte | bool | peer provides datetime
|
|
5 | 1 byte | bool | peer provides gps
|
|
6 | 1 byte | bool | peer provides accelerometer
|
|
7 | 1 byte | bool | peer provides gyroscope
|
|
8 | 1 byte | bool | peer provides magnetometer
|
|
9 | 1 byte | bool | peer provides euler orientation
|
|
10 | 1 byte | bool | peer provides quaternion orientation
|
|
"""
|
|
mutex.lock()
|
|
if packet.get(4) == 0xf1:
|
|
awaiting_sensors[peer_ip] = "VEHICLE"
|
|
elif packet.get(4) == 0x23:
|
|
awaiting_sensors[peer_ip] = "LIDAR_2D"
|
|
signal_queue.append(["sensor_request_received", peer_ip, awaiting_sensors[peer_ip]])
|
|
mutex.unlock()
|
|
|
|
func _parse_timestamp_slice(packet: PackedByteArray):
|
|
var hour = float(packet.decode_u8(0))
|
|
var minute = float(packet.decode_u8(1))
|
|
var second = float(packet.decode_u8(2))
|
|
var nanosecond = float(packet.decode_u32(3)) * pow(10,-9)
|
|
return [hour, minute, second, nanosecond]
|
|
|
|
func _handle_heartbeat_packet(peer_ip: String, packet: PackedByteArray):
|
|
#print('heartbeat')
|
|
var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11))
|
|
mutex.lock()
|
|
signal_queue.append(["on_time_data_received", timestamp_list])
|
|
mutex.unlock()
|
|
#print(hour, ' ', minute, ' ', second)
|
|
|
|
func _handle_time_packet(peer_ip: String, packet: PackedByteArray):
|
|
#print('time')
|
|
var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11))
|
|
mutex.lock()
|
|
signal_queue.append(["on_time_data_received", timestamp_list])
|
|
mutex.unlock()
|
|
#print(timestamp_list)
|
|
|
|
func _handle_position_packet(peer_ip: String, packet: PackedByteArray):
|
|
#print('gps')
|
|
var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11))
|
|
mutex.lock()
|
|
signal_queue.append(["on_time_data_received", timestamp_list])
|
|
mutex.unlock()
|
|
var lat: float = (float(packet.decode_u64(11)) / 10.**9) - 90
|
|
var lon: float = (float(packet.decode_u64(19)) / 10.**9) - 180
|
|
var alt_neg = packet.decode_u8(27)
|
|
var alt_m: float = packet.decode_u32(28) / 1000.0 * (-1. ** alt_neg)
|
|
var geoid_neg = packet.decode_u8(32)
|
|
var goid_m: float = packet.decode_u16(33) / 100 * (-1. ** geoid_neg)
|
|
var speed_neg = packet.decode_u8(35)
|
|
var speed_kmh: float = packet.decode_u32(36) / 1000000.0 * (-1 ** speed_neg)
|
|
var pdop: float = packet.decode_u32(40) / 1000.0
|
|
var hdop: float = packet.decode_u32(44) / 1000.0
|
|
var vdop: float = packet.decode_u32(49) / 1000.0
|
|
mutex.lock()
|
|
signal_queue.append(["gps_data_received", peer_ip, timestamp_list, [lat, lon, alt_m, goid_m], speed_kmh, [pdop, hdop, vdop]])
|
|
signal_queue.append(["raw_data_received", peer_ip, "GPS", timestamp_list, {"lat": lat, "lon": lon, "alt_m": alt_m, "speed_kmh": speed_kmh, "pdop": pdop, "hdop": hdop, "vdop": vdop}])
|
|
mutex.unlock()
|
|
|
|
func _handle_gps_sats_packet(peer_ip: String, packet: PackedByteArray):
|
|
var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11))
|
|
mutex.lock()
|
|
signal_queue.append(["on_time_data_received", timestamp_list])
|
|
mutex.unlock()
|
|
var ga = packet.decode_u8(11)
|
|
var gb = packet.decode_u8(12)
|
|
var gi = packet.decode_u8(13)
|
|
var gl = packet.decode_u8(14)
|
|
var gp = packet.decode_u8(15)
|
|
var gq = packet.decode_u8(16)
|
|
var gn = packet.decode_u8(17)
|
|
var num_entries = packet.decode_u8(18)
|
|
var sats_offset = 19
|
|
var el_offset = 4
|
|
var az_offset = 5
|
|
var sat_data = []
|
|
for i in range(num_entries):
|
|
var start_offset = sats_offset + (i * 7)
|
|
var sat_name = packet.slice(start_offset, start_offset + el_offset).get_string_from_ascii()
|
|
var elevation = packet.decode_u8(start_offset+el_offset)
|
|
var azimuth = packet.decode_u16(start_offset+az_offset)
|
|
sat_data.append([sat_name, elevation, azimuth])
|
|
mutex.lock()
|
|
signal_queue.append(["sat_data_received", peer_ip, timestamp_list, [ga, gb, gi, gl, gp, gq, gn], sat_data])
|
|
mutex.unlock()
|
|
|
|
func _handle_accel_packet(peer_ip: String, packet: PackedByteArray):
|
|
var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11))
|
|
mutex.lock()
|
|
signal_queue.append(["on_time_data_received", timestamp_list])
|
|
mutex.unlock()
|
|
var neg = packet.decode_u8(11)
|
|
var x = (packet.decode_s32(12) / 1000000.0) * (-1 ** neg)
|
|
neg = packet.decode_u8(16)
|
|
var y = (packet.decode_s32(17) / 1000000.0) * (-1 ** neg)
|
|
neg = packet.decode_u8(21)
|
|
var z = (packet.decode_s32(22) / 1000000.0) * (-1 ** neg)
|
|
|
|
neg = packet.decode_u8(26)
|
|
var lin_x = (packet.decode_s32(27) / 1000000.0) * (-1 ** neg)
|
|
neg = packet.decode_u8(31)
|
|
var lin_y = (packet.decode_s32(32) / 1000000.0) * (-1 ** neg)
|
|
neg = packet.decode_u8(36)
|
|
var lin_z = (packet.decode_s32(37) / 1000000.0) * (-1 ** neg)
|
|
|
|
neg = packet.decode_u8(41)
|
|
var g_x = (packet.decode_s32(42) / 1000000.0) * (-1 ** neg)
|
|
neg = packet.decode_u8(46)
|
|
var g_y = (packet.decode_s32(47) / 1000000.0) * (-1 ** neg)
|
|
neg = packet.decode_u8(51)
|
|
var g_z = (packet.decode_s32(52) / 1000000.0) * (-1 ** neg)
|
|
|
|
mutex.lock()
|
|
signal_queue.append(["accelerometer_data_received", peer_ip, timestamp_list, Vector3(x, y, z), Vector3(lin_x, lin_y, lin_z), Vector3(g_x, g_y, g_z)])
|
|
mutex.unlock()
|
|
|
|
func _handle_magnetic_packet(peer_ip: String, packet: PackedByteArray):
|
|
var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11))
|
|
mutex.lock()
|
|
signal_queue.append(["on_time_data_received", timestamp_list])
|
|
mutex.unlock()
|
|
var neg = packet.decode_u8(11)
|
|
var x = (packet.decode_u32(12) / 1000.0) * (-1 ** neg)
|
|
neg = packet.decode_u8(16)
|
|
var y = (packet.decode_u32(17) / 1000.0) * (-1 ** neg)
|
|
neg = packet.decode_u8(21)
|
|
var z = (packet.decode_u32(22) / 1000.0) * (-1 ** neg)
|
|
mutex.lock()
|
|
signal_queue.append(["magnetometer_data_received", peer_ip, timestamp_list, Vector3(x, y, z)])
|
|
mutex.unlock()
|
|
|
|
func _handle_lidar_2d_packet(peer_ip: String, packet: PackedByteArray):
|
|
var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11))
|
|
mutex.lock()
|
|
signal_queue.append(["on_time_data_received", timestamp_list])
|
|
mutex.unlock()
|
|
var point_count = packet.decode_u8(11)
|
|
var point_data = []
|
|
for idx in range(point_count):
|
|
var offset = idx * 7
|
|
var angle: float = float(packet.decode_u32(12 + offset)) / 10000
|
|
var distance: float = float(packet.decode_u16(12 + offset + 4)) / 1000
|
|
var intensity: int = packet.decode_u8(12 + offset + 6)
|
|
point_data.append([angle, distance, intensity])
|
|
signal_queue.append(["lidar_2d_data_received", peer_ip, timestamp_list, point_count, point_data])
|
|
|
|
func _handle_public_key_packet(peer_ip: String, packet: PackedByteArray):
|
|
var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11))
|
|
mutex.lock()
|
|
signal_queue.append(["on_time_data_received", timestamp_list])
|
|
mutex.unlock()
|
|
|
|
|
|
func _handle_euler_packet(peer_ip: String, packet: PackedByteArray):
|
|
var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11))
|
|
mutex.lock()
|
|
signal_queue.append(["on_time_data_received", timestamp_list])
|
|
mutex.unlock()
|
|
|
|
|
|
func _handle_quaternion_packet(peer_ip: String, packet: PackedByteArray):
|
|
var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11))
|
|
mutex.lock()
|
|
signal_queue.append(["on_time_data_received", timestamp_list])
|
|
mutex.unlock()
|
|
|
|
|
|
func _handle_racebox_message(delta, packet):
|
|
#print(packet.size())
|
|
var year = packet.decode_u16(10)
|
|
var month = packet.decode_u8(12)
|
|
var day = packet.decode_u8(13)
|
|
var hour = packet.decode_u8(14)
|
|
var minute = packet.decode_u8(15)
|
|
var second = packet.decode_u8(16)
|
|
var dt_valid_flags = _to_binary(packet.decode_u8(17))
|
|
var valid_date = int(dt_valid_flags[-1]) == 1
|
|
var valid_time = int(dt_valid_flags[-2]) == 1
|
|
var dt_fully_resolved = int(dt_valid_flags[-3]) == 1
|
|
var valid_mag_dec = int(dt_valid_flags[-4]) == 1
|
|
var time_accuracy_ns = packet.decode_u32(18)
|
|
var nano_second = packet.decode_u32(22)
|
|
var fix_status_flags = _to_binary(packet.decode_u8(27))
|
|
var dt_flags = _to_binary(packet.decode_u8(28))
|
|
var num_sv = packet.decode_u8(29)
|
|
var lon_deg = float(packet.decode_u32(30)) / (10**7)
|
|
var lat_deg = float(packet.decode_u32(34)) / (10**7)
|
|
var wgs_alt_m = float(packet.decode_u32(38)) / (10**3) + 6378.137
|
|
var msl_alt_m = float(packet.decode_u32(42)) / (10**3) + 6378.137
|
|
var horz_accuracy_m = float(packet.decode_u32(46)) / (10**3)
|
|
var vert_accuracy_m = float(packet.decode_u32(50)) / (10**3)
|
|
var speed_mps = float(packet.decode_s32(54)) / (10**3)
|
|
var heading_deg = float(packet.decode_s32(58)) / (10**5)
|
|
var speed_accuracy_mps = float(packet.decode_u32(62)) / (10**3)
|
|
var heading_accuracy_deg = float(packet.decode_u32(66)) / (10**5)
|
|
var pdop = float(packet.decode_u16(70)) / 100
|
|
var lat_lon_flags = _to_binary(packet.decode_u8(72))
|
|
var battery_status = _to_binary(packet.decode_u8(73))
|
|
var g_force_x = packet.decode_s16(74) # milli-g
|
|
var g_force_y = packet.decode_s16(76) # milli-g
|
|
var g_force_z = packet.decode_s16(78) # milli-g
|
|
var rot_rate_x_dps = float(packet.decode_s16(80)) / 100
|
|
var rot_rate_y_dps = float(packet.decode_s16(82)) / 100
|
|
var rot_rate_z_dps = float(packet.decode_s16(84)) / 100
|
|
if num_sv > 0:
|
|
var lon_rad = deg_to_rad(lon_deg)
|
|
var lat_rad = deg_to_rad(lat_deg)
|
|
var sensor_pos_x = wgs_alt_m * sin(lon_rad) * cos(lat_rad)
|
|
var sensor_pos_y = wgs_alt_m * sin(lon_rad) * sin(lat_rad)
|
|
var sensor_pos_z = wgs_alt_m * cos(lon_rad)
|
|
var camera_pos_x = (wgs_alt_m+1) * sin(lon_rad) * cos(lat_rad)
|
|
var camera_pos_y = (wgs_alt_m+1) * sin(lon_rad) * sin(lat_rad)
|
|
var camera_pos_z = (wgs_alt_m+1) * cos(lon_rad)
|
|
camera_position_updated.emit(camera_pos_x, camera_pos_y, camera_pos_z)
|
|
camera_rotation_updated.emit(deg_to_rad(rot_rate_x_dps) * delta, deg_to_rad(rot_rate_y_dps) * delta, deg_to_rad(rot_rate_z_dps) * delta)
|
|
sensor_point_generated.emit(sensor_pos_x, sensor_pos_y, sensor_pos_z)
|
|
else:
|
|
print(Time.get_datetime_string_from_system())
|
|
print("GPS has no sources")
|
|
|
|
func _to_binary(intValue: int) -> String:
|
|
var bin_str: String = ""
|
|
while intValue > 0:
|
|
bin_str = str(intValue & 1) + bin_str
|
|
intValue = intValue >> 1
|
|
return bin_str
|
|
|