diff --git a/Main.tscn b/Main.tscn index 20af8f4..a213d37 100644 --- a/Main.tscn +++ b/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"] diff --git a/data_recorder_node.gd b/data_recorder_node.gd new file mode 100644 index 0000000..de89b76 --- /dev/null +++ b/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]) + diff --git a/data_recorder_node.gd.uid b/data_recorder_node.gd.uid new file mode 100644 index 0000000..3942677 --- /dev/null +++ b/data_recorder_node.gd.uid @@ -0,0 +1 @@ +uid://cb0e5702tjrqc diff --git a/networking/scripts/udp_server.gd b/networking/scripts/udp_server.gd index fc6da97..960a2e3 100644 --- a/networking/scripts/udp_server.gd +++ b/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": - 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]) + elif signal_data[0] == "raw_data_received": + DataSignals.raw_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 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 + var data = [ + packet.decode_u64(11), + packet.decode_u64(19), + packet.decode_u32(28) * (-1. ** packet.decode_u8(27)), + packet.decode_u16(33)* (-1. ** packet.decode_u8(32)), + packet.decode_u32(36) * (-1 ** packet.decode_u8(35)), + packet.decode_u32(40), + packet.decode_u32(44), + packet.decode_u32(49) + ] 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}]) + signal_queue.append(["raw_data_received", peer_ip, "GPS", timestamp_list, data]) 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): diff --git a/sensor_approval/SensorApproval.tscn b/sensor_approval/SensorApproval.tscn index 2b8b251..c7b641f 100644 --- a/sensor_approval/SensorApproval.tscn +++ b/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"] diff --git a/sensor_approval/data_column.tscn b/sensor_approval/data_column.tscn index 27981ad..11a1f81 100644 --- a/sensor_approval/data_column.tscn +++ b/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 diff --git a/sensor_approval/sensor_approval.gd b/sensor_approval/sensor_approval.gd index ecf485f..841b59f 100644 --- a/sensor_approval/sensor_approval.gd +++ b/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) diff --git a/sensors/vehicle_sensor_node.gd b/sensors/vehicle_sensor_node.gd index 280dcbd..b6ccdd8 100644 --- a/sensors/vehicle_sensor_node.gd +++ b/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): - # coordinates = [lat rad, lon rad, alt m, geoid diff m] - # dop = [pdop, hdop, vdop] +func _on_raw_data_received(source_ip: String, sensor_type: String, timestamp_list: Array, data: Array): 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) - speed_data.insert(insert_idx, speed_kmh) + altitudes.insert(insert_idx, altitude) + 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): - if source_ip != ip_address: - return +func _on_accel_data_received(timestamp_list: Array, accel: Vector3, linear_accel: Vector3, g_accel: Vector3): 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 diff --git a/signals/data_signals.gd b/signals/data_signals.gd index e47d0dc..69eb675 100644 --- a/signals/data_signals.gd +++ b/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)