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

Changed to vehicle sensor instead of single sensor types

Browse Source
This commit is contained in:
smithserver_admin
2026-07-04 23:49:20 -06:00
parent 55522fc6fc
commit 10e9a0f5df
147 changed files with 523 additions and 8763 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sensors/accelerometer_sensor_node.gd
-30
View File
@@ -1,30 +0,0 @@
extends BaseSensorNode
class_name AccelSensorNode
var accel_vector: Vector3 = Vector3.ZERO
var linear_accel_vector: Vector3 = Vector3.ZERO
var g_accel_vector: Vector3 = Vector3.ZERO
var DOWN = Vector2(0, -1.)
# Called when the node enters the scene tree for the first time.
func _ready() -> void:
super._ready()
DataSignals.accelerometer_data_received.connect(_on_data_received)
func _on_data_received(source_ip: String, timestamp_list: Array, accel: Array, linear_accel: Array, g_accel: Array):
if source_ip != ip_address:
return
var msg_timestamp = timestamp_list[0] * 3600 + timestamp_list[1] * 60 + timestamp_list[2] + timestamp_list[3]
if not Constants.is_timestamp_newer(timestamp, msg_timestamp):
return
timestamp = msg_timestamp
Signals.linear_acceleration_updated.emit(ip_address, linear_accel)
var x_z = Vector2(g_accel[0], g_accel[2]).normalized()
var y_z = Vector2(g_accel[1], g_accel[2]).normalized()
var pitch = DOWN.angle_to(x_z)
var roll = y_z.angle_to(DOWN)
SensorSignals.pitch_updated.emit(ip_address, pitch)
SensorSignals.roll_updated.emit(ip_address, roll)
sensors/accelerometer_sensor_node.gd.uid
-1
View File
@@ -1 +0,0 @@
uid://b8tbp8y3ic2hi
sensors/base_sensor_node.gd
+9 -1
View File
@@ -5,7 +5,15 @@ var max_measurement_size: float = 10
@export var ip_address: String
var id: String = UUID.new().as_string()
var timestamp: float = 0.
var max_measurement_history_s: float = 10.
var latest_timestamp: float = 0.
var sensor_class: String:
get:
return _get_sensor_class()
func _ready() -> void:
pass
func _get_sensor_class():
return null
sensors/magnetometer_sensor_node.gd
-24
View File
@@ -1,24 +0,0 @@
extends BaseSensorNode
class_name MagnetSensorNode
var heading_rad: float = 0
# Called when the node enters the scene tree for the first time.
func _ready() -> void:
super._ready()
DataSignals.magnetometer_data_received.connect(_on_data_received)
func _on_data_received(source_ip: String, timestamp_list: Array, mag_vec: Array):
if source_ip != ip_address:
return
var msg_timestamp = timestamp_list[0] * 3600 + timestamp_list[1] * 60 + timestamp_list[2] + timestamp_list[3]
if not Constants.is_timestamp_newer(timestamp, msg_timestamp):
return
timestamp = msg_timestamp
var mag = Vector2(mag_vec[0], mag_vec[1])
mag.normalized()
heading_rad = atan2(-mag[1], mag[0])
Signals.sensor_mag_updated.emit(ip_address)
Signals.heading_updated.emit(ip_address, heading_rad)
sensors/magnetometer_sensor_node.gd.uid
-1
View File
@@ -1 +0,0 @@
uid://gwr7babnfojp
sensors/pnt_sensor_node.gd
-56
View File
@@ -1,56 +0,0 @@
extends BaseSensorNode
class_name PntSensorNode
var lat_rad: float = 0.
var lon_rad: float = 0.
var alt_km: float = 0.
var speed_kmh: float = 0.
var gps_position = Vector3(1., 1., 1.)
var speed_data: Array = []
var lat_data: Array = []
var lon_data: Array = []
var alt_data: Array = []
var position_data: Array = []
var measurement_times: Array = []
# Called when the node enters the scene tree for the first time.
func _ready() -> void:
super._ready()
DataSignals.pnt_data_received.connect(_on_data_received)
func _on_data_received(source_ip: String, timestamp_list: Array, coordinates: Array, pnt_speed_kmh: float, dop: Array):
if source_ip != ip_address:
return
var msg_timestamp = timestamp_list[0] * 3600. + timestamp_list[1] * 60. + timestamp_list[2] + timestamp_list[3]
var insert_idx = Bisect.bisect(measurement_times, msg_timestamp)
measurement_times.insert(insert_idx, msg_timestamp)
speed_kmh = pnt_speed_kmh
speed_data.insert(insert_idx, speed_data)
lat_rad = deg_to_rad(coordinates[0])
lat_data.insert(insert_idx, lat_rad)
lon_rad = deg_to_rad(coordinates[1])
lon_data.insert(insert_idx, lon_rad)
alt_km = (coordinates[2] + coordinates[3]) / 1000
alt_data.insert(insert_idx, alt_km)
var radius_km = (coordinates[2] + coordinates[3] + Constants.EARTH_RADIUS_M) / 1000.
var pos_x = radius_km * sin(lon_rad) * cos(lat_rad)
var pos_z = radius_km * sin(lon_rad) * sin(lat_rad)
var pos_y = radius_km * cos(lon_rad)
gps_position = Vector3(pos_x, pos_y, pos_z)
position_data.insert(insert_idx, gps_position)
Signals.sensor_pnt_updated.emit(ip_address, self)
Signals.altitude_updated.emit(ip_address, alt_km)
Signals.position_updated.emit(ip_address, gps_position)
SensorSignals.speed_updated.emit(ip_address, speed_kmh)
while measurement_times.size() > max_measurement_size:
measurement_times.pop_back()
speed_data.pop_back()
lat_data.pop_back()
lon_data.pop_back()
alt_data.pop_back()
position_data.pop_back()
sensors/pnt_sensor_node.gd.uid
-1
View File
@@ -1 +0,0 @@
uid://ddxquur7spfsf
sensors/satelites_sensor_node.gd
+2 -2
View File
@@ -11,9 +11,9 @@ func _on_data_received(source_ip: String, timestamp_list: Array, sat_counts: Arr
if source_ip != ip_address:
return
var msg_timestamp = timestamp_list[0] * 3600 + timestamp_list[1] * 60 + timestamp_list[2] + timestamp_list[3]
if not Constants.is_timestamp_newer(timestamp, msg_timestamp):
if not Constants.is_timestamp_newer(latest_timestamp, msg_timestamp):
return
timestamp = msg_timestamp
latest_timestamp = msg_timestamp
var sat_ids = []
for sat_data in sat_positions:
sat_ids.append(sat_data[0])
sensors/sensor_spawner.gd
+4 -19
View File
@@ -1,7 +1,6 @@
extends Node3D
class_name SensorSpawner
@export var sensor_scene: PackedScene
var vehicle: VehicleNode
# Called when the node enters the scene tree for the first time.
@@ -10,21 +9,7 @@ func _ready() -> void:
SensorSignals.new_sensor_connected.connect(_on_new_sensor_connected)
func _on_new_sensor_connected(sensor_ip: String, provider_data: Array) -> void:
var sensor_suite = sensor_scene.instantiate()
sensor_suite.ip_address = sensor_ip
vehicle.sensor_suites[sensor_ip] = sensor_suite
if provider_data[0]:
sensor_suite.provides_pnt = true
sensor_suite.create_pnt()
sensor_suite.create_sats()
if provider_data[1]:
sensor_suite.provides_accel = true
sensor_suite.create_accel()
if provider_data[2]:
sensor_suite.provides_gyro = true
sensor_suite.create_gyro()
if provider_data[3]:
sensor_suite.provides_mag = true
sensor_suite.create_mag()
add_child(sensor_suite)
vehicle.add_sensor_node(sensor_suite)
var sensor_node = VehicleSensorNode.new()
sensor_node.ip_address = sensor_ip
add_child(sensor_node)
vehicle.add_sensor_node(sensor_node)
sensors/sensor_suite.tscn
-4
View File
@@ -1,7 +1,3 @@
[gd_scene format=3 uid="uid://trl3gugdy3hw"]
[ext_resource type="Script" uid="uid://xm4wqedyteei" path="res://sensors/sensor_suite_node.gd" id="1_1i1as"]
[node name="SensorSuite" type="Node3D" unique_id=1330923752]
script = ExtResource("1_1i1as")
metadata/_custom_type_script = "uid://xm4wqedyteei"
sensors/sensor_suite_node.gd
-54
View File
@@ -1,54 +0,0 @@
extends Node3D
class_name SensorSuiteNode
@export var ip_address: String
var provides_pnt: bool = false
var provides_accel: bool = false
var provides_gyro: bool = false
var provides_mag: bool = false
var sensors = {}
"""
sensor_suite.provides_pnt = true
sensor_suite.create_pnt()
sensor_suite.create_sats()
if provider_data[1]:
sensor_suite.provides_accel = true
sensor_suite.create_accel()
if provider_data[2]:
sensor_suite.provides_gyro = true
sensor_suite.create_gyro()
if provider_data[3]:
sensor_suite.provides_mag = true
sensor_suite.create_mag()
"""
func create_pnt():
var sensor = PntSensorNode.new()
sensor.ip_address = ip_address
sensors[sensor.id] = sensor
add_child(sensor)
func create_sats():
var sensor = SatelliteSensorNode.new()
sensor.ip_address = ip_address
sensors[sensor.id] = sensor
add_child(sensor)
func create_accel():
var sensor = AccelSensorNode.new()
sensor.ip_address = ip_address
sensors[sensor.id] = sensor
add_child(sensor)
func create_gyro():
return
func create_mag():
var sensor = MagnetSensorNode.new()
sensor.ip_address = ip_address
sensors[sensor.id] = sensor
add_child(sensor)
sensors/sensor_suite_node.gd.uid
-1
View File
@@ -1 +0,0 @@
uid://xm4wqedyteei
sensors/vehicle_sensor_node.gd
+190
View File
@@ -0,0 +1,190 @@
extends BaseSensorNode
class_name VehicleSensorNode
"""
Vehicle sensor can provide the following data fields:
- GPS
- Contributing satellites
- Accelerometer
- Magnetometer
- Gyroscope
"""
var max_data_length: int = 10
var gps_timestamps = []
var coordinates = []
var gps_positions = []
var altitudes = []
var altitude_m:
get:
return _get_current_altitude()
var speed_data = []
var speed_kmh:
get:
return _get_current_speed()
static var DOWN = Vector2(0, -1.)
var accel_timestamps = []
var g_vecs = []
var linear_vecs = []
var pitch_data = []
var pitch_rad: float
var roll_data = []
var roll_rad: float
var mag_timestamps = []
var mag_vecs = []
var heading_data = []
var heading_rad: float
func _ready() -> void:
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)
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]
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() == max_data_length and not TimeHelpers.is_timestamp_newer_24h(gps_timestamps[-1], timestamp):
return
var insert_idx = Bisect.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])
var pos_z = radius_km * sin(new_coordinates[1]) * sin(new_coordinates[0])
var pos_y = radius_km * cos(new_coordinates[1])
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)
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()
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
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 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)
var roll = y_z.angle_to(DOWN)
accel_timestamps.insert(insert_idx, timestamp)
linear_vecs.insert(insert_idx, linear_accel)
g_vecs.insert(insert_idx, g_accel)
pitch_data.insert(insert_idx, pitch)
roll_data.insert(insert_idx, roll)
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()
_update_avg_pitch()
_update_avg_roll()
func _on_mag_data_received(source_ip: String, timestamp_list: Array, mag_vec: 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 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 mag_2d = Vector2(mag_vec.x, mag_vec.y)
mag_2d.normalized()
var new_heading_rad = atan2(-mag_2d.y, mag_2d.x)
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()
func _on_gyro_data_received(source_ip: String, timestamp: float):
if source_ip != ip_address:
return
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)
func _get_current_speed():
if gps_timestamps.size() < 3:
return null
var speed_dx = []
var speed_dx_sum = 0
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)
func _update_avg_pitch():
var pitch_sum = 0
for i in range(accel_timestamps.size()):
pitch_sum += pitch_data[i]
pitch_rad = pitch_sum / pitch_data.size()
func _update_avg_roll():
var roll_sum = 0
for i in range(accel_timestamps.size()):
roll_sum += roll_data[i]
roll_rad = roll_sum / roll_data.size()
func _update_avg_heading():
var heading_sum = 0
for i in range(mag_timestamps.size()):
heading_sum += heading_data[i]
heading_rad = heading_sum / heading_data.size()
sensors/vehicle_sensor_node.gd.uid
+1
View File
@@ -0,0 +1 @@
uid://c30nkkqcp0orm