Main.tscn

@@ -11,6 +11,7 @@
 [ext_resource type="PackedScene" uid="uid://crpqmmpxfe2yi" path="res://views/3d/View3D.tscn" id="10_wkp8b"]
 [ext_resource type="PackedScene" uid="uid://cf2vkjpwwdog7" path="res://vehicle/vehicle.tscn" id="11_5vvyt"]
 [ext_resource type="PackedScene" uid="uid://blijvkgrhoas" path="res://settings/SettingsMenu.tscn" id="11_20pc6"]
+[ext_resource type="Script" uid="uid://cb0e5702tjrqc" path="res://data_recorder_node.gd" id="12_5vvyt"]
 
 [node name="Main" type="Node" unique_id=247156581]
 
@@ -51,5 +52,9 @@ text = "Settings"
 
 [node name="SettingsMenu" parent="Settings" unique_id=1877869509 instance=ExtResource("11_20pc6")]
 
+[node name="DataRecorderNode" type="Node" parent="." unique_id=504334504]
+script = ExtResource("12_5vvyt")
+metadata/_custom_type_script = "uid://cb0e5702tjrqc"
+
 [connection signal="pressed" from="Settings/OpenSettings" to="ViewController" method="_on_open_settings_pressed"]
 [connection signal="pressed" from="Settings/OpenSettings" to="Settings" method="_on_settings_opened"]

data_recorder_node.gd

@@ -0,0 +1,95 @@
+extends Node
+class_name DataRecorderNode
+
+var MAX_SENSOR_DATA_SIZE = 524288000
+
+var cached_data = []
+
+var thread: Thread
+var mutex: Mutex
+var exit_thread = false
+
+var timer = 0
+var timer_target = 15
+
+var target_dir = "user://sensor_data/"
+
+# Called when the node enters the scene tree for the first time.
+func _ready() -> void:
+	DataSignals.raw_data_received.connect(_on_raw_data_received)
+	thread = Thread.new()
+	mutex = Mutex.new()
+	thread.start(_thread_func)
+	DirAccess.make_dir_absolute(target_dir)
+
+func _exit_tree() -> void:
+	mutex.lock()
+	exit_thread = true
+	mutex.unlock()
+	thread.wait_to_finish()
+
+func _process(delta: float) -> void:
+	mutex.lock()
+	timer += delta
+	mutex.unlock()
+
+func _on_raw_data_received(source_ip: String, data_type: String, timestamp_list: Array, data_dict: Dictionary):
+	cached_data.append([source_ip, data_type, timestamp_list, data_dict])
+
+func _thread_func():
+	while true:
+		mutex.lock()
+		var should_exit = exit_thread
+		mutex.unlock()
+		if should_exit:
+			break
+		mutex.lock()
+		var should_write_data = (timer >= timer_target)
+		mutex.unlock()
+		if not should_write_data:
+			continue
+		mutex.lock()
+		timer = 0
+		var data_copy = null
+		if cached_data.size() > 0:
+			data_copy = cached_data.duplicate(true)
+			cached_data.clear()
+		mutex.unlock()
+		if data_copy != null:
+			_write_data(data_copy)
+		_delete_old_data()
+
+func _write_data(data_to_write: Array):
+	var current_file_path = target_dir + str(Time.get_unix_time_from_system()) + "_sensor_data.csv"
+	var file = FileAccess.open(current_file_path, FileAccess.WRITE)
+	for data in data_to_write:
+		var line = str(data[0]) + ',' + str(data[1]) + ','
+		for time_data in data[2]:
+			line += str(time_data) + ','
+		for value in data[3]:
+			line += str(value) + ','
+		file.store_line(line)
+	file.close()
+	
+func _delete_old_data():
+	var files: Array = []
+	var dir = DirAccess.open(target_dir)
+	dir.list_dir_begin()
+	while true:
+		var file_name = dir.get_next()
+		if file_name == "":
+			break
+		elif file_name.begins_with("."):
+			continue
+		files.append(file_name)
+	dir.list_dir_end()
+	files.sort()
+	var cutoff_idx = 0
+	var file_size_sum = 0
+	for idx in range(files.size(), 0, -1):
+		file_size_sum += FileAccess.get_size(target_dir + files[idx-1])
+		if file_size_sum >= MAX_SENSOR_DATA_SIZE:
+			cutoff_idx = idx
+	for i in range(cutoff_idx):
+		DirAccess.remove_absolute(target_dir+files[i])
+	

data_recorder_node.gd.uid

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

networking/scripts/udp_server.gd

@@ -48,16 +48,8 @@ func _process(delta: float) -> void:
 			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":
+		elif signal_data[0] == "raw_data_received":
-			DataSignals.gps_data_received.emit(signal_data[1], signal_data[2], signal_data[3], signal_data[4], signal_data[5])
+			DataSignals.raw_data_received.emit(signal_data[1], signal_data[2], signal_data[3], signal_data[4])
-		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:
@@ -226,20 +218,18 @@ func _handle_position_packet(peer_ip: String, packet: PackedByteArray):
 	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 data = [
-	var lon: float = (float(packet.decode_u64(19)) / 10.**9) - 180
+		packet.decode_u64(11),
-	var alt_neg = packet.decode_u8(27)
+		packet.decode_u64(19),
-	var alt_m: float = packet.decode_u32(28) / 1000.0 * (-1. ** alt_neg)
+		packet.decode_u32(28) * (-1. ** packet.decode_u8(27)),
-	var geoid_neg = packet.decode_u8(32)
+		packet.decode_u16(33)* (-1. ** packet.decode_u8(32)),
-	var goid_m: float = packet.decode_u16(33) / 100 * (-1. ** geoid_neg)
+		packet.decode_u32(36) * (-1 ** packet.decode_u8(35)),
-	var speed_neg = packet.decode_u8(35)
+		packet.decode_u32(40),
-	var speed_kmh: float = packet.decode_u32(36) / 1000000.0 * (-1 ** speed_neg)
+		packet.decode_u32(44),
-	var pdop: float = packet.decode_u32(40) / 1000.0
+		packet.decode_u32(49)
-	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, data])
-	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):
@@ -259,14 +249,17 @@ func _handle_gps_sats_packet(peer_ip: String, packet: PackedByteArray):
 	var el_offset = 4
 	var az_offset = 5
 	var sat_data = []
+	var data = [ga, gb, gi, gl, gp, gq, gn]
 	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])
+		data.append(sat_name)
+		data.append(elevation)
+		data.append(azimuth)
 	mutex.lock()
-	signal_queue.append(["sat_data_received", peer_ip, timestamp_list, [ga, gb, gi, gl, gp, gq, gn], sat_data])
+	signal_queue.append(["raw_data_received", peer_ip, "SATS", timestamp_list, data])
 	mutex.unlock()
 	
 func _handle_accel_packet(peer_ip: String, packet: PackedByteArray):
@@ -275,28 +268,34 @@ func _handle_accel_packet(peer_ip: String, packet: PackedByteArray):
 	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)
+	var x = packet.decode_s32(12) * (-1 ** neg)
 	neg = packet.decode_u8(16)
-	var y = (packet.decode_s32(17) / 1000000.0) * (-1 ** neg)
+	var y = packet.decode_s32(17) * (-1 ** neg)
 	neg = packet.decode_u8(21)
-	var z = (packet.decode_s32(22) / 1000000.0) * (-1 ** neg)
+	var z = packet.decode_s32(22) * (-1 ** neg)
 	
 	neg = packet.decode_u8(26)
-	var lin_x = (packet.decode_s32(27) / 1000000.0) * (-1 ** neg)
+	var lin_x = packet.decode_s32(27) * (-1 ** neg)
 	neg = packet.decode_u8(31)
-	var lin_y = (packet.decode_s32(32) / 1000000.0) * (-1 ** neg)
+	var lin_y = packet.decode_s32(32) * (-1 ** neg)
 	neg = packet.decode_u8(36)
-	var lin_z = (packet.decode_s32(37) / 1000000.0) * (-1 ** neg)
+	var lin_z = packet.decode_s32(37) * (-1 ** neg)
 	
 	neg = packet.decode_u8(41)
-	var g_x = (packet.decode_s32(42) / 1000000.0) * (-1 ** neg)
+	var g_x = packet.decode_s32(42) * (-1 ** neg)
 	neg = packet.decode_u8(46)
-	var g_y = (packet.decode_s32(47) / 1000000.0) * (-1 ** neg)
+	var g_y = packet.decode_s32(47) * (-1 ** neg)
 	neg = packet.decode_u8(51)
-	var g_z = (packet.decode_s32(52) / 1000000.0) * (-1 ** neg)
+	var g_z = packet.decode_s32(52) * (-1 ** neg)
+	
+	var data = [
+		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)])
+	signal_queue.append(["raw_data_received", peer_ip, "ACCEL", timestamp_list, data])
 	mutex.unlock()
 	
 func _handle_magnetic_packet(peer_ip: String, packet: PackedByteArray):

sensor_approval/SensorApproval.tscn

@@ -19,6 +19,7 @@ layout_mode = 2
 
 [node name="Label" type="Label" parent="HBoxContainer" unique_id=1098572929]
 layout_mode = 2
+theme_override_font_sizes/font_size = 32
 text = "127.0.0.1"
 
 [node name="Control" type="Control" parent="HBoxContainer" unique_id=1047175521]
@@ -28,6 +29,7 @@ layout_mode = 2
 [node name="Approve" type="Button" parent="HBoxContainer" unique_id=2039550355]
 layout_mode = 2
 mouse_filter = 1
+theme_override_font_sizes/font_size = 32
 text = "Approve"
 
 [node name="Control2" type="Control" parent="HBoxContainer" unique_id=225178386]
@@ -37,6 +39,7 @@ layout_mode = 2
 [node name="Decline" type="Button" parent="HBoxContainer" unique_id=1630362833]
 layout_mode = 2
 mouse_filter = 1
+theme_override_font_sizes/font_size = 32
 text = "Decline"
 
 [connection signal="pressed" from="HBoxContainer/Approve" to="." method="_on_approve_pressed"]

sensor_approval/data_column.tscn

@@ -6,8 +6,10 @@ offset_bottom = 50.0
 
 [node name="DataLabel" type="Label" parent="." unique_id=1663588703]
 layout_mode = 2
+theme_override_font_sizes/font_size = 16
 horizontal_alignment = 1
 
 [node name="DataValue" type="Label" parent="." unique_id=1789333906]
 layout_mode = 2
+theme_override_font_sizes/font_size = 16
 horizontal_alignment = 1

sensor_approval/sensor_approval.gd

@@ -14,7 +14,7 @@ var data_columns: Array
 
 
 func _setup_data_columns():
-	var container = get_node('HBoxContainer')
+	var container: HBoxContainer = get_node('HBoxContainer')
 	for i in range(provided_data.size()):
 		var spacer: Control = container.add_spacer(false)
 		spacer.size = Vector2(2, 0)

sensors/vehicle_sensor_node.gd

@@ -49,20 +49,52 @@ var heading_rad: float
 
 func _ready() -> void:
 	super._ready()
-	DataSignals.gps_data_received.connect(_on_gps_data_received)
+	DataSignals.raw_data_received.connect(_on_raw_data_received)
 	#DataSignals.sat_data_received.connect(_on_sat_data_received)
 	DataSignals.accelerometer_data_received.connect(_on_accel_data_received)
-	DataSignals.magnetometer_data_received.connect(_on_mag_data_received)
 	#DataSignals.gyroscope_data_received.connect(_on_gyro_data_received)
 
 func _get_sensor_class():
 	return "VEHICLE"
 
-func _on_gps_data_received(source_ip: String, timestamp_list: Array, new_coordinates: Array, speed_kmh: float, dop: Array):
+func _on_raw_data_received(source_ip: String, sensor_type: String, timestamp_list: Array, data: Array):
-	# coordinates = [lat rad, lon rad, alt m, geoid diff m]
-	# dop = [pdop, hdop, vdop]
 	if source_ip != ip_address:
 		return
+	if sensor_type == "GPS":
+		var parsed_coordinates = [
+			(float(data[0]) / 10.**9) - 90,
+			(float(data[1]) / 10.**9) - 180,
+			(float(data[2]) / 1000.),
+			(float(data[3]) / 100.),
+		]
+		var speed = float(data[4]) / 1000000.
+		var dop = [
+			float(data[5]) / 1000.,
+			float(data[6]) / 1000.,
+			float(data[7]) / 1000.,
+		]
+		_on_gps_data_received(timestamp_list, parsed_coordinates, speed, dop)
+	if sensor_type == "ACCEL":
+		var accel = Vector3(
+			float(data[0]) / 1000000.,
+			float(data[1]) / 1000000.,
+			float(data[2]) / 1000000.
+		)
+		var linear_accel = Vector3(
+			float(data[3]) / 1000000.,
+			float(data[4]) / 1000000.,
+			float(data[5]) / 1000000.
+		)
+		var g_accel = Vector3(
+			float(data[6]) / 1000000.,
+			float(data[7]) / 1000000.,
+			float(data[8]) / 1000000.
+		)
+		_on_accel_data_received(timestamp_list, accel, linear_accel, g_accel)
+	
+func _on_gps_data_received(timestamp_list: Array, new_coordinates: Array, speed: float, dop: Array):
+	# coordinates = [lat rad, lon rad, alt m, geoid diff m]
+	# dop = [pdop, hdop, vdop]
 	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()
@@ -73,7 +105,7 @@ func _on_gps_data_received(source_ip: String, timestamp_list: Array, new_coordin
 	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)
-	var altitude_m = new_coordinates[2] + new_coordinates[3]
+	var altitude = new_coordinates[2] + new_coordinates[3]
 	var radius_km = (new_coordinates[2] + new_coordinates[3] + Constants.EARTH_RADIUS_M) / 1000.
 	var pos_x = radius_km * sin(new_coordinates[1]) * cos(new_coordinates[0])
 	var pos_z = radius_km * sin(new_coordinates[1]) * sin(new_coordinates[0])
@@ -81,8 +113,8 @@ func _on_gps_data_received(source_ip: String, timestamp_list: Array, new_coordin
 	gps_timestamps.insert(insert_idx, timestamp)
 	coordinates.insert(insert_idx, [new_coordinates[0], new_coordinates[1]])
 	gps_positions.insert(insert_idx, Vector3(pos_x, pos_y, pos_z))
-	altitudes.insert(insert_idx, altitude_m)
+	altitudes.insert(insert_idx, altitude)
-	speed_data.insert(insert_idx, speed_kmh)
+	speed_data.insert(insert_idx, speed)
 	if gps_timestamps.size() > max_data_length:
 		gps_timestamps.pop_back()
 		coordinates.pop_back()
@@ -96,9 +128,7 @@ func _on_sat_data_received(source_ip: String, timestamp: float):
 	if source_ip != ip_address:
 		return
 
-func _on_accel_data_received(source_ip: String, timestamp_list: Array, accel: Vector3, linear_accel: Vector3, g_accel: Vector3):
+func _on_accel_data_received(timestamp_list: Array, accel: Vector3, linear_accel: Vector3, g_accel: Vector3):
-	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()
@@ -166,9 +196,8 @@ func get_derrivative(dataset: Array) -> Array:
 	var deltas = []
 	var delta_sum = 0
 	for i in range(dataset.size()-1):
-		var delta = dataset[i+1] - dataset[i]
 		deltas.append(dataset[i+1] - dataset[i])
-	return [delta_sum / deltas.size(), deltas]
+	return [float(delta_sum) / deltas.size(), deltas]
 
 func get_array_avg(dataset: Array):
 	var data_sum = 0

signals/data_signals.gd

@@ -1,7 +1,7 @@
 extends Node
 
 
-signal raw_data_received(source_ip: String, data_type: String, timestamp_list: Array, data_dict: Dictionary)
+signal raw_data_received(source_ip: String, data_type: String, timestamp_list: Array, data: Array)
 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)