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
sensors/vehicle_sensor_node.gd
+18
View File
@@ -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)