6FPS/car-os
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Changed to vehicle sensor instead of single sensor types

Browse Source
This commit is contained in:
smithserver_admin
2026-07-04 23:49:20 -06:00
parent 55522fc6fc
commit 10e9a0f5df
147 changed files with 523 additions and 8763 deletions
Download Patch File Download Diff File Expand all files Collapse all files
networking/scripts/udp_server.gd
+102 -25
View File
@@ -12,6 +12,12 @@ var awaiting_sensors = {}
var invalid_sensors = {}
var valid_sensors = {}
var thread: Thread
var mutex: Mutex
var exit_thread = false
var signal_queue = []
func _ready():
ServerSignals.sensor_approved.connect(_on_sensor_approved)
ServerSignals.sensor_declined.connect(_on_sensor_declined)
@@ -19,33 +25,73 @@ func _ready():
if not server.is_listening():
print('Unable to listen on port: ', port)
print('Listening on port: ', port)
mutex = Mutex.new()
thread = Thread.new()
thread.start(_thread_function)
func _process(delta: float) -> void:
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])
func _exit_tree() -> void:
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)
SensorSignals.new_sensor_connected.emit(sensor_ip, valid_sensors[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 _process(delta):
_process_connections(delta)
func _thread_function():
while true:
mutex.lock()
var should_exit = exit_thread
mutex.unlock()
if should_exit:
break
_process_connections()
func _process_connections(delta):
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())
_handle_six_fps_message(peer, packet)
else:
_handle_new_connection(delta, peer, packet)
_handle_new_connection(peer, packet)
func _handle_new_connection(delta: float, peer: PacketPeerUDP, packet: PackedByteArray):
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 |
@@ -66,14 +112,9 @@ func _handle_new_connection(delta: float, peer: PacketPeerUDP, packet: PackedByt
packet[packet.size()-1] = ck_b
"""
_handle_six_fps_message(peer, packet)
elif packet.get(0) == 0xb5 and packet.get(1) == 0x62:
_handle_racebox_message(delta, packet)
else:
print("Received: '%s' %s:%s" % [packet.get_string_from_utf8(), peer.get_packet_ip(), peer.get_packet_port()])
for i in range(0, valid_sensors.size()):
pass # Do something with the connected peers.
func _calculate_checksum(packet) -> Array:
var ck_a = 0
var ck_b = 0
@@ -92,7 +133,9 @@ func _handle_six_fps_message(peer: PacketPeerUDP, packet: PackedByteArray):
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):
@@ -133,6 +176,7 @@ func _handle_new_peer_request(peer_ip: String, packet: PackedByteArray):
9 | 1 byte | bool | peer provides euler orientation
10 | 1 byte | bool | peer provides quaternion orientation
"""
mutex.lock()
awaiting_sensors[peer_ip] = [
#packet.decode_u8(4),
packet.decode_u8(5),
@@ -142,31 +186,38 @@ func _handle_new_peer_request(peer_ip: String, packet: PackedByteArray):
#packet.decode_u8(9),
#packet.decode_u8(10),
]
print('sensor request')
ServerSignals.sensor_request_received.emit(peer_ip, awaiting_sensors[peer_ip])
signal_queue.append(["sensor_request_received", peer_ip, awaiting_sensors[peer_ip]])
mutex.unlock()
func _parse_timestamp_slice(packet: PackedByteArray):
var hour = packet.decode_u8(0)
var minute = packet.decode_u8(1)
var second = packet.decode_u8(2)
var nanosecond = packet.decode_u32(3) * pow(10,-9)
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 hour = packet.decode_u8(4)
var minute = packet.decode_u8(5)
var second = packet.decode_u8(6)
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)
@@ -178,10 +229,15 @@ func _handle_position_packet(peer_ip: String, packet: PackedByteArray):
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
DataSignals.pnt_data_received.emit(peer_ip, timestamp_list, [lat, lon, alt_m, goid_m], speed_kmh, [pdop, hdop, vdop])
mutex.lock()
signal_queue.append(["gps_data_received", peer_ip, timestamp_list, [lat, lon, alt_m, goid_m], speed_kmh, [pdop, hdop, 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)
@@ -200,10 +256,15 @@ func _handle_gps_sats_packet(peer_ip: String, packet: PackedByteArray):
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])
DataSignals.sat_data_received.emit(peer_ip, timestamp_list, [ga, gb, gi, gl, gp, gq, gn], sat_data)
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)
@@ -225,28 +286,44 @@ func _handle_accel_packet(peer_ip: String, packet: PackedByteArray):
neg = packet.decode_u8(51)
var g_z = (packet.decode_s32(52) / 1000000.0) * (-1 ** neg)
DataSignals.accelerometer_data_received.emit(peer_ip, timestamp_list, [x, y, z], [lin_x, lin_y, lin_z], [g_x, g_y, g_z])
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)
DataSignals.magnetometer_data_received.emit(peer_ip, timestamp_list, [x, y, z])
mutex.lock()
signal_queue.append(["magnetometer_data_received", peer_ip, timestamp_list, Vector3(x, y, z)])
mutex.unlock()
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):