Adding lidar sensor handling

networking/scripts/udp_server.gd

@@ -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()

project.godot

@@ -36,6 +36,7 @@ file_logging/enable_file_logging=true
 
 window/size/viewport_width=800
 window/size/viewport_height=800
+window/size/mode=4
 
 [physics]
 
@@ -44,3 +45,4 @@ window/size/viewport_height=800
 [rendering]
 
 rendering_device/driver.windows="d3d12"
+renderer/rendering_method="mobile"

sensors/lidar_2d_sensor_node.gd

@@ -1,9 +1,14 @@
 extends BaseSensorNode
 class_name Lidar2DSensorNode
 
+
 # Called when the node enters the scene tree for the first time.
 func _ready() -> void:
-	pass # Replace with function body.
+	super._ready()
+	DataSignals.lidar_2d_data_received.connect(_on_lidar_data_received)
+	DataSignals.gps_data_received.connect(_on_gps_data_received)
+	DataSignals.accelerometer_data_received.connect(_on_accel_data_received)
+	DataSignals.gyroscope_data_received.connect(_on_gyro_data_received)
 
 
 # Called every frame. 'delta' is the elapsed time since the previous frame.
@@ -12,3 +17,14 @@ func _process(delta: float) -> void:
 
 func _get_sensor_class():
 	return "LIDAR_2D"
+
+func _on_lidar_data_received(sensor_ip: String, timestamp_list: Array, lidar_point_count: int, lidar_data: Array):pass
+
+func _on_gps_data_received(source_ip: String, timestamp_list: Array, new_coordinates: Array, speed_kmh: float, dop: Array):
+	pass
+	
+func _on_accel_data_received(source_ip: String, timestamp_list: Array, accel: Vector3, linear_accel: Vector3, g_accel: Vector3):
+	pass
+
+func _on_gyro_data_received(source_ip: String, timestamp_list: Array, gyro_data: Array):
+	pass

sensors/vehicle_sensor_node.gd

@@ -48,6 +48,7 @@ var heading_rad: float
 
 
 func _ready() -> void:
+	super._ready()
 	DataSignals.gps_data_received.connect(_on_gps_data_received)
 	#DataSignals.sat_data_received.connect(_on_sat_data_received)
 	DataSignals.accelerometer_data_received.connect(_on_accel_data_received)
@@ -63,6 +64,12 @@ func _on_gps_data_received(source_ip: String, timestamp_list: Array, new_coordin
 	if source_ip != ip_address:
 		return
 	var timestamp: float = 3600.*timestamp_list[0] + 60.*timestamp_list[1] + timestamp_list[2] + timestamp_list[3]
+	if gps_timestamps.size() > 0 and (timestamp - gps_timestamps[-1]) < -1:
+		gps_timestamps.clear()
+		coordinates.clear()
+		gps_positions.clear()
+		altitudes.clear()
+		speed_data.clear()
 	if gps_timestamps.size() == max_data_length and not TimeHelpers.is_timestamp_newer_24h(gps_timestamps[-1], timestamp):
 		return
 	var insert_idx = Bisect.reverse_bisect(gps_timestamps, timestamp)
@@ -93,6 +100,13 @@ func _on_accel_data_received(source_ip: String, timestamp_list: Array, accel: Ve
 	if source_ip != ip_address:
 		return
 	var timestamp: float = 3600.*timestamp_list[0] + 60.*timestamp_list[1] + timestamp_list[2] + timestamp_list[3]
+	if accel_timestamps.size() > 0 and (timestamp - accel_timestamps[-1]) < -1:
+		accel_timestamps.clear()
+		linear_vecs.clear()
+		g_vecs.clear()
+		pitch_data.clear()
+		roll_data.clear()
+		lateral_g_data.clear()
 	if accel_timestamps.size() == max_data_length and not TimeHelpers.is_timestamp_newer_24h(accel_timestamps[-1], timestamp):
 		return
 	var insert_idx = Bisect.reverse_bisect(accel_timestamps, timestamp)
@@ -122,6 +136,10 @@ func _on_mag_data_received(source_ip: String, timestamp_list: Array, mag_vec: Ve
 	if source_ip != ip_address:
 		return
 	var timestamp: float = 3600.*timestamp_list[0] + 60.*timestamp_list[1] + timestamp_list[2] + timestamp_list[3]
+	if mag_timestamps.size() > 0 and (timestamp - mag_timestamps[-1]) < -1:
+		mag_timestamps.clear()
+		mag_vecs.clear()
+		heading_data.clear()
 	if mag_timestamps.size() == max_data_length and not TimeHelpers.is_timestamp_newer_24h(mag_timestamps[-1], timestamp):
 		return
 	var insert_idx = Bisect.reverse_bisect(mag_timestamps, timestamp)

signals/data_signals.gd

@@ -1,9 +1,11 @@
 extends Node
 
 
+signal raw_data_received(source_ip: String, data_type: String, timestamp_list: Array, data_dict: Dictionary)
 signal time_data_received(source_ip: String, timestamp_list: Array)
 signal gps_data_received(source_ip: String, timestamp_list: Array, coordinates: Array, speed_kmh: float, dop: Array)
 signal sat_data_received(source_ip: String, timestamp: float, sat_count: Array, sat_position: Array)
 signal accelerometer_data_received(source_ip: String, timestamp: float, accel: Vector3, linear_accel: Vector3, g_accel: Vector3)
 signal magnetometer_data_received(source_ip: String, timestamp: float, mag_vec: Vector3)
 signal gyroscope_data_received(source_ip: String, timestamp: float, data: Array)
+signal lidar_2d_data_received(sensor_ip: String, timestamp_list: Array, lidar_point_count: int, lidar_data: Array)

views/2d/View2D.tscn

@@ -1,6 +1,5 @@
 [gd_scene format=3 uid="uid://c2yga5koqwo72"]
 
-[ext_resource type="PackedScene" uid="uid://bbdbyjievjpsq" path="res://widgets/compass/compass_widget_scene.tscn" id="1_fyqef"]
 [ext_resource type="Script" uid="uid://ba7ta7jjrjuhg" path="res://views/2d/view_2d.gd" id="1_n6fjn"]
 [ext_resource type="PackedScene" uid="uid://b71kuj4yole3j" path="res://widgets/satellites/satellite_widget.tscn" id="2_3b2b5"]
 [ext_resource type="PackedScene" uid="uid://m14p8xxs14rf" path="res://widgets/altitude/altitude_widget.tscn" id="3_g6l6n"]
@@ -14,9 +13,6 @@ script = ExtResource("1_n6fjn")
 
 [node name="HUD" type="CanvasLayer" parent="." unique_id=1826106128]
 
-[node name="Compass" parent="HUD" unique_id=2062816665 instance=ExtResource("1_fyqef")]
-position = Vector2(104, 486)
-
 [node name="TotalSatCount" parent="HUD" unique_id=931018379 instance=ExtResource("2_3b2b5")]
 visible = false
 scale = Vector2(0.05, 0.05)
@@ -64,10 +60,10 @@ scale = Vector2(0.05, 0.05)
 sat_category = "QZSS"
 
 [node name="AltitudeWidget" parent="HUD" unique_id=1004159593 instance=ExtResource("3_g6l6n")]
-position = Vector2(170, 481)
+position = Vector2(233, 479)
 
 [node name="AccelerometerWidget" parent="HUD" unique_id=1057256615 instance=ExtResource("4_qui0h")]
-position = Vector2(675, 400)
+position = Vector2(673, 302)
 
 [node name="RollWidget" parent="HUD" unique_id=1446458625 instance=ExtResource("6_s8d37")]
 position = Vector2(407, 267)

views/2d/test_lidar.gd

@@ -0,0 +1,39 @@
+extends Node2D
+
+var line: Line2D
+
+var points_angle = []
+var points_distance = []
+var timer_static = 60
+var timer = 0.
+
+# Called when the node enters the scene tree for the first time.
+func _ready() -> void:
+	line = get_node('Line2D')
+	DataSignals.lidar_2d_data_received.connect(_on_lidar_data_received)
+
+
+# Called every frame. 'delta' is the elapsed time since the previous frame.
+func _process(delta: float) -> void:
+	timer += delta
+	if timer > timer_static:
+		timer = 0
+		points_angle.clear()
+		points_distance.clear()
+		for i in line.points:
+			print(i - Vector2(400, 400))
+		print()
+	_update_line()
+
+func _on_lidar_data_received(sensor_ip: String, timestamp_list: Array, lidar_point_count: int, lidar_data: Array):
+	for point in lidar_data:
+		var insert_idx = Bisect.bisect(points_angle, point[0])
+		points_angle.insert(insert_idx, point[0])
+		points_distance.insert(insert_idx, point[1])
+
+func _update_line():
+	line.clear_points()
+	for idx in range(points_angle.size()):
+		var x = 4*points_distance[idx] * cos(deg_to_rad(points_angle[idx])) + 400
+		var y = 4*points_distance[idx] * sin(deg_to_rad(points_angle[idx])) + 400
+		line.add_point(Vector2(x, y))

views/2d/test_lidar.gd.uid

@@ -0,0 +1 @@
+uid://cei6fgt3rsc2c

views/2d/test_lidar.tscn

@@ -0,0 +1,13 @@
+[gd_scene format=3 uid="uid://ch0lyqkkxaem"]
+
+[ext_resource type="Script" uid="uid://cei6fgt3rsc2c" path="res://views/2d/test_lidar.gd" id="1_8h8gx"]
+
+[node name="TestLidar" type="Node2D" unique_id=948964638]
+script = ExtResource("1_8h8gx")
+
+[node name="Camera2D" type="Camera2D" parent="." unique_id=60160156]
+position = Vector2(400, 400)
+
+[node name="Line2D" type="Line2D" parent="." unique_id=1424623383]
+closed = true
+width = 1.0

widgets/accelerometer/accelerometer_widget.tscn

@@ -7,6 +7,16 @@
 script = ExtResource("1_hqf45")
 metadata/_custom_type_script = "uid://cr4oydwvmm28y"
 
+[node name="Label" type="Label" parent="." unique_id=749031600]
+custom_minimum_size = Vector2(200, 0)
+offset_left = -100.0
+offset_top = 90.0
+offset_right = 100.0
+offset_bottom = 178.0
+theme_override_font_sizes/font_size = 64
+text = "10.0 g"
+horizontal_alignment = 1
+
 [node name="Panel" type="Panel" parent="." unique_id=1154284634]
 visible = false
 offset_left = -103.0
@@ -32,17 +42,13 @@ default_color = Color(0, 1, 0, 1)
 begin_cap_mode = 2
 end_cap_mode = 2
 
-[node name="Label" type="Label" parent="." unique_id=749031600]
-offset_right = 40.0
-offset_bottom = 23.0
-
 [node name="MovableControl" type="Control" parent="." unique_id=345939378 node_paths=PackedStringArray("move_target")]
 layout_mode = 3
 anchors_preset = 0
 offset_left = -103.0
 offset_top = -103.0
 offset_right = 106.0
-offset_bottom = 106.0
+offset_bottom = 183.0
 mouse_filter = 1
 script = ExtResource("2_pog3i")
 move_target = NodePath("..")

widgets/accelerometer/scripts/accelerometer_widget_node.gd

@@ -24,8 +24,9 @@ func _update_data():
 		return
 	var lateral_gs = host_vehicle.lateral_accel
 	var accel_g = lateral_gs.length() / Constants.g
-	var x_vec = Vector2(lateral_gs.x, 0)
+	var end_point = Vector2(lateral_gs.y, -lateral_gs.x)
-	accel_vector.points = [origin, lateral_gs * 4]
+	var forward = Vector2(0, -lateral_gs.x)
-	x_vector.points = [origin, x_vec * 4]
+	accel_vector.points = [origin, end_point * 4]
-	y_vector.points = [x_vec * 4, lateral_gs * 4]
+	x_vector.points = [forward * 4, end_point * 4]
+	y_vector.points = [origin, forward * 4]
 	label.text = str(round(accel_g*10)/10) + ' g'

widgets/altitude/altitude_widget.tscn

@@ -8,21 +8,21 @@ script = ExtResource("1_hmdxp")
 metadata/_custom_type_script = "uid://b3rfje1auw6to"
 
 [node name="Panel" type="Panel" parent="." unique_id=1393400967]
-offset_right = 460.0
+offset_right = 300.0
 offset_bottom = 120.0
 
 [node name="Label" type="Label" parent="." unique_id=1070194714]
-offset_right = 460.0
+offset_right = 300.0
 offset_bottom = 120.0
-theme_override_font_sizes/font_size = 80
+theme_override_font_sizes/font_size = 64
-text = "Alt: 99999 ft"
+text = "99999 ft"
 horizontal_alignment = 1
 vertical_alignment = 1
 
 [node name="MovableControl" type="Control" parent="." unique_id=1186292353 node_paths=PackedStringArray("move_target")]
 layout_mode = 3
 anchors_preset = 0
-offset_right = 460.0
+offset_right = 300.0
 offset_bottom = 120.0
 mouse_filter = 1
 script = ExtResource("2_34sps")

widgets/altitude/scripts/altitude_widget_node.gd

@@ -14,4 +14,4 @@ func _ready() -> void:
 func _update_data():
 	if label_node == null:
 		return
-	label_node.text = 'Alt: ' + str(int(round(host_vehicle.altitude_m * Constants.M2FT))) + ' ft'
+	label_node.text = str(int(round(host_vehicle.altitude_m * Constants.M2FT))) + ' ft'

widgets/lidar_2d/lidar_2d.tscn

@@ -0,0 +1,3 @@
+[gd_scene format=3 uid="uid://dhxuhkb8inhqa"]
+
+[node name="Lidar2d" type="Node3D" unique_id=679038150]

widgets/rotation/pitch_widget.tscn

@@ -26,7 +26,7 @@ offset_left = -50.0
 offset_top = -22.0
 offset_right = 50.0
 offset_bottom = 23.0
-theme_override_font_sizes/font_size = 32
+theme_override_font_sizes/font_size = 48
 horizontal_alignment = 1
 
 [node name="Line2D" type="Line2D" parent="." unique_id=1285997474]

widgets/rotation/roll_widget.tscn

@@ -26,7 +26,7 @@ offset_left = -50.0
 offset_top = -22.0
 offset_right = 50.0
 offset_bottom = 23.0
-theme_override_font_sizes/font_size = 32
+theme_override_font_sizes/font_size = 48
 horizontal_alignment = 1
 
 [node name="Line2D" type="Line2D" parent="." unique_id=1446923009]

widgets/rotation/scripts/pitch_widget_node.gd

@@ -16,4 +16,4 @@ func _update_data():
 	if sprite == null or label == null:
 		return
 	sprite.rotation = host_vehicle.pitch_rad
-	label.text = str(round(rad_to_deg(host_vehicle.pitch_rad)*10)/10) + " \u00B0"
+	label.text = str(int(rad_to_deg(host_vehicle.pitch_rad))) + " \u00B0"

widgets/rotation/scripts/roll_widget_node.gd

@@ -15,4 +15,4 @@ func _update_data():
 	if sprite == null or label == null:
 		return
 	sprite.rotation = host_vehicle.roll_rad
-	label.text = str(abs(round(rad_to_deg(host_vehicle.roll_rad)*10)/10)) + " \u00B0"
+	label.text = str(abs(int(rad_to_deg(host_vehicle.roll_rad)))) + " \u00B0"

widgets/speed/speed_widget.tscn

@@ -14,7 +14,7 @@ offset_bottom = 40.0
 [node name="SpeedLabel" type="Label" parent="VBoxContainer" unique_id=2101712075]
 custom_minimum_size = Vector2(150, 0)
 layout_mode = 2
-theme_override_font_sizes/font_size = 80
+theme_override_font_sizes/font_size = 64
 text = "10"
 horizontal_alignment = 1