initial commit

sensors/accelerometer_sensor_node.gd

@@ -0,0 +1,30 @@
+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

@@ -0,0 +1 @@
+uid://b8tbp8y3ic2hi

sensors/base_sensor_node.gd

@@ -0,0 +1,11 @@
+extends Node
+class_name BaseSensorNode
+
+var max_measurement_size: float = 10
+@export var ip_address: String
+
+var id: String = UUID.new().as_string()
+var timestamp: float = 0.
+
+func _ready() -> void:
+	pass

sensors/base_sensor_node.gd.uid

@@ -0,0 +1 @@
+uid://duxnoiiytlenq

sensors/magnetometer_sensor_node.gd

@@ -0,0 +1,24 @@
+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

@@ -0,0 +1 @@
+uid://gwr7babnfojp

sensors/pnt_sensor_node.gd

@@ -0,0 +1,56 @@
+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

@@ -0,0 +1 @@
+uid://ddxquur7spfsf

sensors/satelites_sensor_node.gd

@@ -0,0 +1,23 @@
+extends BaseSensorNode
+class_name SatelliteSensorNode
+
+
+# Called when the node enters the scene tree for the first time.
+func _ready() -> void:
+	super._ready()
+	DataSignals.sat_data_received.connect(_on_data_received)
+	
+func _on_data_received(source_ip: String, timestamp_list: Array, sat_counts: Array, sat_positions: 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 sat_ids = []
+	for sat_data in sat_positions:
+		sat_ids.append(sat_data[0])
+		#sat_position = _calc_sat_position(sat_data[1], sat_data[2])
+	Signals.sensor_sats_updated.emit(ip_address, self)
+	Signals.sat_count_updated.emit(ip_address, sat_counts)
+	#Signals.sat_position_updated.emit(ip_address, sat_positions)

sensors/satelites_sensor_node.gd.uid

@@ -0,0 +1 @@
+uid://cag2l70d821g5

sensors/sensor_spawner.gd

@@ -0,0 +1,30 @@
+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.
+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_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)

sensors/sensor_spawner.gd.uid

@@ -0,0 +1 @@
+uid://bflafrhk4eq4w

sensors/sensor_suite.tscn

@@ -0,0 +1,7 @@
+[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

@@ -0,0 +1,54 @@
+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

@@ -0,0 +1 @@
+uid://xm4wqedyteei