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

Bunch of changes

Browse Source
This commit is contained in:
smithserver_admin
2026-08-01 18:19:21 -06:00
parent 04f3e2ba07
commit 7102175711
9 changed files with 184 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Main.tscn
+5
View File
@@ -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
+95
View File
@@ -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
+1
View File
@@ -0,0 +1 @@
uid://cb0e5702tjrqc
networking/scripts/udp_server.gd
+34 -35
View File
@@ -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):
sensor_approval/SensorApproval.tscn
+3
View File
@@ -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
+2
View File
@@ -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
+1 -1
View File
@@ -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
+42 -13
View File
@@ -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
signals/data_signals.gd
+1 -1
View File
@@ -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)