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

Adding lidar sensor handling

Browse Source
This commit is contained in:
smithserver_admin
2026-07-08 20:58:21 -06:00
parent 8fc5088c01
commit 04f3e2ba07
19 changed files with 145 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
networking/scripts/udp_server.gd
+21 -4
View File
@@ -56,6 +56,8 @@ func _process(delta: float) -> void:
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:
@@ -167,6 +169,8 @@ func _handle_six_fps_message(peer: PacketPeerUDP, packet: PackedByteArray):
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
@@ -186,9 +190,6 @@ func _handle_new_peer_request(peer_ip: String, packet: PackedByteArray):
10 | 1 byte | bool | peer provides quaternion orientation
"""
mutex.lock()
print(packet)
print(packet.get(4))
print(0xf1)
if packet.get(4) == 0xf1:
awaiting_sensors[peer_ip] = "VEHICLE"
elif packet.get(4) == 0x23:
@@ -238,6 +239,7 @@ func _handle_position_packet(peer_ip: String, packet: PackedByteArray):
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):
@@ -311,7 +313,22 @@ func _handle_magnetic_packet(peer_ip: String, packet: PackedByteArray):
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()