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

Change how different sensor types ar handled

Browse Source
This commit is contained in:
smithserver_admin
2026-07-06 13:57:09 -06:00
parent 6eb2993391
commit 8fc5088c01
21 changed files with 320 additions and 124 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sensors/lidar_2d_sensor_node.gd
+14
View File
@@ -0,0 +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.
# Called every frame. 'delta' is the elapsed time since the previous frame.
func _process(delta: float) -> void:
pass
func _get_sensor_class():
return "LIDAR_2D"
sensors/lidar_2d_sensor_node.gd.uid
+1
View File
@@ -0,0 +1 @@
uid://c00jgww14wp8w
sensors/sensor_spawner.gd
+8 -2
View File
@@ -8,8 +8,14 @@ func _ready() -> void:
vehicle = get_parent()
SensorSignals.new_sensor_connected.connect(_on_new_sensor_connected)
func _on_new_sensor_connected(sensor_ip: String, provider_data: Array) -> void:
var sensor_node = VehicleSensorNode.new()
func _on_new_sensor_connected(sensor_ip: String, sensor_type: String) -> void:
var sensor_node: BaseSensorNode
if sensor_type == "VEHICLE":
sensor_node = VehicleSensorNode.new()
elif sensor_type == "LIDAR_2D":
sensor_node = Lidar2DSensorNode.new()
else:
return
sensor_node.ip_address = sensor_ip
add_child(sensor_node)
vehicle.add_sensor_node(sensor_node)
sensors/vehicle_sensor_node.gd
+106 -50
View File
@@ -16,10 +16,12 @@ var gps_timestamps = []
var coordinates = []
var gps_positions = []
var altitudes = []
var alt_coefficients = []
var altitude_m:
get:
return _get_current_altitude()
var speed_data = []
var speed_coefficients = []
var speed_kmh:
get:
return _get_current_speed()
@@ -32,6 +34,12 @@ var pitch_data = []
var pitch_rad: float
var roll_data = []
var roll_rad: float
var lateral_g_data = []
var x_coefficients = []
var y_coefficients = []
var lateral_accel:
get:
return _get_current_gs()
var mag_timestamps = []
var mag_vecs = []
@@ -57,7 +65,7 @@ func _on_gps_data_received(source_ip: String, timestamp_list: Array, new_coordin
var timestamp: float = 3600.*timestamp_list[0] + 60.*timestamp_list[1] + timestamp_list[2] + timestamp_list[3]
if gps_timestamps.size() == max_data_length and not TimeHelpers.is_timestamp_newer_24h(gps_timestamps[-1], timestamp):
return
var insert_idx = Bisect.bisect(gps_timestamps, timestamp)
var insert_idx = Bisect.reverse_bisect(gps_timestamps, timestamp)
var altitude_m = 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])
@@ -69,11 +77,13 @@ func _on_gps_data_received(source_ip: String, timestamp_list: Array, new_coordin
altitudes.insert(insert_idx, altitude_m)
speed_data.insert(insert_idx, speed_kmh)
if gps_timestamps.size() > max_data_length:
gps_timestamps.pop_front()
coordinates.pop_front()
gps_positions.pop_front()
altitudes.pop_front()
speed_data.pop_front()
gps_timestamps.pop_back()
coordinates.pop_back()
gps_positions.pop_back()
altitudes.pop_back()
speed_data.pop_back()
_update_alt_coefficients()
_update_speed_coefficients()
func _on_sat_data_received(source_ip: String, timestamp: float):
if source_ip != ip_address:
@@ -85,7 +95,7 @@ func _on_accel_data_received(source_ip: String, timestamp_list: Array, accel: Ve
var timestamp: float = 3600.*timestamp_list[0] + 60.*timestamp_list[1] + timestamp_list[2] + timestamp_list[3]
if accel_timestamps.size() == max_data_length and not TimeHelpers.is_timestamp_newer_24h(accel_timestamps[-1], timestamp):
return
var insert_idx = Bisect.bisect(accel_timestamps, timestamp)
var insert_idx = Bisect.reverse_bisect(accel_timestamps, timestamp)
var x_z = Vector2(g_accel.x, g_accel.z).normalized()
var y_z = Vector2(g_accel.y, g_accel.z).normalized()
var pitch = DOWN.angle_to(x_z)
@@ -95,14 +105,18 @@ func _on_accel_data_received(source_ip: String, timestamp_list: Array, accel: Ve
g_vecs.insert(insert_idx, g_accel)
pitch_data.insert(insert_idx, pitch)
roll_data.insert(insert_idx, roll)
var lateral_g = Vector2(linear_accel.x, linear_accel.y)
lateral_g_data.insert(insert_idx, lateral_g.normalized()*lateral_g.length())
if accel_timestamps.size() > max_data_length:
accel_timestamps.pop_front()
linear_vecs.pop_front()
g_vecs.pop_front()
pitch_data.pop_front()
roll_data.pop_front()
accel_timestamps.pop_back()
linear_vecs.pop_back()
g_vecs.pop_back()
pitch_data.pop_back()
roll_data.pop_back()
lateral_g_data.pop_back()
_update_avg_pitch()
_update_avg_roll()
_update_g_coefficients()
func _on_mag_data_received(source_ip: String, timestamp_list: Array, mag_vec: Vector3):
if source_ip != ip_address:
@@ -110,64 +124,106 @@ func _on_mag_data_received(source_ip: String, timestamp_list: Array, mag_vec: Ve
var timestamp: float = 3600.*timestamp_list[0] + 60.*timestamp_list[1] + timestamp_list[2] + timestamp_list[3]
if mag_timestamps.size() == max_data_length and not TimeHelpers.is_timestamp_newer_24h(mag_timestamps[-1], timestamp):
return
var insert_idx = Bisect.bisect(mag_timestamps, timestamp)
var insert_idx = Bisect.reverse_bisect(mag_timestamps, timestamp)
var mag_2d = Vector2(mag_vec.x, mag_vec.y)
mag_2d.normalized()
var new_heading_rad = atan2(-mag_2d.y, mag_2d.x)
if new_heading_rad < 0:
new_heading_rad += 2*PI
mag_timestamps.insert(insert_idx, timestamp)
mag_vecs.insert(insert_idx, mag_vec)
heading_data.insert(insert_idx, new_heading_rad)
if mag_timestamps.size() > max_data_length:
mag_timestamps.pop_front()
mag_vecs.pop_front()
heading_data.pop_front()
_update_avg_heading()
mag_timestamps.pop_back()
mag_vecs.pop_back()
heading_data.pop_back()
func _on_gyro_data_received(source_ip: String, timestamp: float):
if source_ip != ip_address:
return
func get_derrivative(dataset: Array) -> Array:
if dataset.size() < 2:
return [0, []]
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]
func get_array_avg(dataset: Array):
var data_sum = 0
for i in dataset:
data_sum += i
return data_sum / dataset.size()
func get_coefficients(dataset: Array, order: int):
if order == 0:
return []
if dataset.size() <= order:
order = dataset.size() - 1
var coefficients = []
var tmp_dataset = dataset
var dx_result: Array
for i in range(order):
dx_result = get_derrivative(tmp_dataset)
coefficients.append(dx_result[0])
tmp_dataset = dx_result[1]
return coefficients
func _update_alt_coefficients():
if gps_timestamps.size() < 2:
return
alt_coefficients = get_coefficients(altitudes, 2)
func _get_current_altitude():
if gps_timestamps.size() < 3:
return null
var altitude_dx = []
var alt_dx_sum = 0
var current_timestamp = TimeHelpers.get_current_sensor_time()
for i in range(gps_timestamps.size()-1, 0, -1):
var alt_dx = altitudes[i] - altitudes[i-1]
alt_dx_sum += alt_dx
altitude_dx.append(alt_dx)
var altitude_dx2 = []
var alt_dx2_sum = 0
for i in range(altitude_dx.size()-1):
var alt_dx2 = altitude_dx[i] - altitude_dx[i+1]
alt_dx2_sum += alt_dx2
altitude_dx2.append(alt_dx2)
var time_diff = max(0, TimeHelpers.calc_time_diff_24h(gps_timestamps[-1], current_timestamp))
var dv = alt_dx2_sum/altitude_dx2.size()
var dx = alt_dx_sum / altitude_dx.size()
return altitudes[-1] + ((dx + (dv * time_diff)) * time_diff)
var time_diff = max(0, TimeHelpers.calc_time_diff_24h(gps_timestamps[0], current_timestamp))
var current_value = altitudes[0]
for idx in range(alt_coefficients.size()):
current_value += alt_coefficients[idx] * (time_diff ** idx+1)
return current_value
func _update_speed_coefficients():
if gps_timestamps.size() < 2:
return
speed_coefficients = get_coefficients(speed_data, 2)
func _get_current_speed():
if gps_timestamps.size() < 3:
return null
var speed_dx = []
var speed_dx_sum = 0
return
var current_timestamp = TimeHelpers.get_current_sensor_time()
for i in range(gps_timestamps.size()-1, 0, -1):
var spd_dx = speed_data[i] - speed_data[i-1]
speed_dx_sum += spd_dx
speed_dx.append(spd_dx)
var speed_dx2 = []
var speed_dx2_sum = 0
for i in range(speed_dx.size()-1):
var spd_dx2 = speed_dx[i] - speed_dx[i+1]
speed_dx2_sum += spd_dx2
speed_dx2.append(spd_dx2)
var time_diff = max(0, TimeHelpers.calc_time_diff_24h(gps_timestamps[-1], current_timestamp))
var dv = speed_dx2_sum/speed_dx2.size()
var dx = speed_dx_sum / speed_dx.size()
return speed_data[-1] + ((dx + (dv * time_diff)) * time_diff)
var current_value = speed_data[0]
for idx in range(speed_coefficients.size()):
current_value += speed_coefficients[idx] * (time_diff ** idx+1)
return current_value
func _update_g_coefficients():
if accel_timestamps.size() < 2:
return
var x_dataset = []
var y_dataset = []
for i in lateral_g_data:
x_dataset.append(i.x)
y_dataset.append(i.y)
x_coefficients = get_coefficients(x_dataset, 3)
y_coefficients = get_coefficients(y_dataset, 3)
func _get_current_gs():
if accel_timestamps.size() < 3:
return
var current_timestamp = TimeHelpers.get_current_sensor_time()
var time_diff = max(0, TimeHelpers.calc_time_diff_24h(accel_timestamps[0], current_timestamp))
var current_x = lateral_g_data[0].x
var current_y = lateral_g_data[0].y
for idx in range(speed_coefficients.size()):
current_x += x_coefficients[idx] * (time_diff ** idx+1)
current_y += y_coefficients[idx] * (time_diff ** idx+1)
return Vector2(current_x, current_y)
func _update_avg_pitch():
var pitch_sum = 0
@@ -183,7 +239,7 @@ func _update_avg_roll():
func _update_avg_heading():
var heading_sum = 0
for i in range(mag_timestamps.size()):
for i in range(min(mag_timestamps.size(), 2)):
heading_sum += heading_data[i]
heading_rad = heading_sum / heading_data.size()