Changed to vehicle sensor instead of single sensor types
This commit is contained in:
@@ -13,35 +13,39 @@ var UUID = preload('res://addons/uuid/uuid.gd')
|
||||
|
||||
var host_id: String = ''
|
||||
@export var host: bool = true
|
||||
var axis_scene_template: PackedScene = preload("res://axis/Axis.tscn")
|
||||
var sensor_suites = {}
|
||||
var sensor_nodes = {}
|
||||
var sensor_classes = {
|
||||
"PNT": [],
|
||||
"ACCEL": [],
|
||||
"GYRO": [],
|
||||
"MAG": []
|
||||
"VEHICLE": [],
|
||||
}
|
||||
var datetime_sensors = {}
|
||||
var gps_sensors = {}
|
||||
var accelerometer_sensors = {}
|
||||
var gyro_sensors = {}
|
||||
var magnet_sensors = {}
|
||||
|
||||
var current_timestamp = 0.
|
||||
var lat_rad: float = 0.
|
||||
var long_rad: float = 0.
|
||||
var alt_km: float = 0.
|
||||
var timer: float = 0.
|
||||
static var timer_1: float = 1.
|
||||
|
||||
var position_3d: Vector3 = Vector3.ZERO
|
||||
var altitude_m: float:
|
||||
get:
|
||||
return _get_altitude_m()
|
||||
|
||||
var heading_rad: float = 0.
|
||||
var speed_kmh: float:
|
||||
get:
|
||||
return _get_speed_kmh()
|
||||
|
||||
var roll_rad: float:
|
||||
get:
|
||||
return _get_roll_rad()
|
||||
|
||||
var pitch_rad: float:
|
||||
get:
|
||||
return _get_pitch_rad()
|
||||
|
||||
var heading_rad: float:
|
||||
get:
|
||||
return _get_heading_rad()
|
||||
|
||||
|
||||
# Called when the node enters the scene tree for the first time.
|
||||
func _ready() -> void:
|
||||
if host:
|
||||
_load_id()
|
||||
Signals.position_updated.connect(_on_pnt_position_updated)
|
||||
print('VehicleNode ready')
|
||||
#var axis_scene = axis_scene_template.instantiate()
|
||||
#axis_scene.axis_length = 2.
|
||||
@@ -50,14 +54,10 @@ func _ready() -> void:
|
||||
|
||||
# Called every frame. 'delta' is the elapsed time since the previous frame.
|
||||
func _process(delta: float) -> void:
|
||||
var time_dict = Time.get_time_dict_from_system(true)
|
||||
var unix_time = Time.get_unix_time_from_system()
|
||||
current_timestamp = time_dict["hour"] * 3600 + time_dict["minute"] * 60 + time_dict["second"]
|
||||
pass
|
||||
|
||||
func _physics_process(delta: float) -> void:
|
||||
pass
|
||||
#_update_position()
|
||||
#_update_rotation()
|
||||
|
||||
func _load_id():
|
||||
var file_path = "user://host_id.idf"
|
||||
@@ -71,51 +71,74 @@ func _load_id():
|
||||
|
||||
func _generate_id():
|
||||
return UUID.new().as_string()
|
||||
|
||||
func _get_altitude_m():
|
||||
var alt_sum: float = 0
|
||||
var alt_count: int = 0
|
||||
for sensor_ip in sensor_classes["VEHICLE"]:
|
||||
var sensor_alt = sensor_nodes[sensor_ip].altitude_m
|
||||
if sensor_alt == null:
|
||||
continue
|
||||
alt_sum += sensor_alt
|
||||
alt_count += 1
|
||||
if alt_count == 0:
|
||||
return 0
|
||||
return alt_sum / alt_count
|
||||
|
||||
func _update_position():
|
||||
var time_offset = 0.
|
||||
var sensor_data_array = []
|
||||
for sensor_id in gps_sensors:
|
||||
var sensor = sensor_suites[sensor_id]
|
||||
var sensor_time_offset = Constants.calc_time_diff(sensor.gps_timestamp, current_timestamp)
|
||||
time_offset += sensor_time_offset
|
||||
sensor_data_array.append([sensor_time_offset, sensor.gps_position])
|
||||
var avg_position = Vector3.ZERO
|
||||
for sensor_data in sensor_data_array:
|
||||
avg_position += (sensor_data[0]/time_offset) * sensor_data[1]
|
||||
position = avg_position
|
||||
#print(position)
|
||||
#print(position.length())
|
||||
var camera: Camera3D = get_node("/root/Main/Node3D/Camera3D")
|
||||
camera.position = position + position.normalized() * 1
|
||||
if camera.position != position:
|
||||
camera.look_at(position, Vector3(0, 1, 0))
|
||||
func _get_speed_kmh():
|
||||
var speed_sum: float = 0
|
||||
var speed_count: int = 0
|
||||
for sensor_ip in sensor_classes["VEHICLE"]:
|
||||
var sensor_speed = sensor_nodes[sensor_ip].speed_kmh
|
||||
if sensor_speed == null:
|
||||
continue
|
||||
speed_sum += sensor_speed
|
||||
speed_count += 1
|
||||
if speed_count == 0:
|
||||
return 0
|
||||
return speed_sum / speed_count
|
||||
|
||||
func _get_roll_rad():
|
||||
var roll_sum: float = 0
|
||||
var roll_count: int = 0
|
||||
for sensor_ip in sensor_classes["VEHICLE"]:
|
||||
var sensor_roll = sensor_nodes[sensor_ip].roll_rad
|
||||
if sensor_roll == null:
|
||||
continue
|
||||
roll_sum += sensor_roll
|
||||
roll_count += 1
|
||||
if roll_count == 0:
|
||||
return 0
|
||||
return roll_sum / roll_count
|
||||
|
||||
func _get_pitch_rad():
|
||||
var pitch_sum: float = 0
|
||||
var pitch_count: int = 0
|
||||
for sensor_ip in sensor_classes["VEHICLE"]:
|
||||
var sensor_pitch= sensor_nodes[sensor_ip].pitch_rad
|
||||
if sensor_pitch == null:
|
||||
continue
|
||||
pitch_sum += sensor_pitch
|
||||
pitch_count += 1
|
||||
if pitch_count == 0:
|
||||
return 0
|
||||
return pitch_sum / pitch_count
|
||||
|
||||
func _update_rotation():
|
||||
if position.length() == 0:
|
||||
func _get_heading_rad():
|
||||
var heading_sum: float = 0
|
||||
var heading_count: int = 0
|
||||
for sensor_ip in sensor_classes["VEHICLE"]:
|
||||
var sensor_heading = sensor_nodes[sensor_ip].heading_rad
|
||||
if sensor_heading == null:
|
||||
continue
|
||||
heading_sum += sensor_heading
|
||||
heading_count += 1
|
||||
if heading_count == 0:
|
||||
return 0
|
||||
return heading_sum / heading_count
|
||||
|
||||
func add_sensor_node(sensor_node: BaseSensorNode):
|
||||
sensor_nodes[sensor_node.ip_address] = sensor_node
|
||||
if sensor_node.sensor_class not in sensor_classes:
|
||||
return
|
||||
var down_vec = -1 * position.normalized()
|
||||
var up_axis = Vector3(0, 1, 0)
|
||||
up_axis = (up_axis * quaternion).normalized()
|
||||
var rot_axis = down_vec.cross(up_axis).normalized()
|
||||
var rot_deg = up_axis.angle_to(down_vec)
|
||||
var rot_quat = Quaternion(rot_axis, rot_deg).normalized()
|
||||
quaternion = (quaternion * rot_quat).normalized()
|
||||
|
||||
func _on_pnt_position_updated(sensor_ip: String, sensor_position_3d: Vector3):
|
||||
if sensor_ip not in sensor_suites:
|
||||
return
|
||||
if sensor_ip not in sensor_classes["PNT"]:
|
||||
return
|
||||
position_3d = sensor_position_3d
|
||||
|
||||
func add_sensor_node(sensor_node: SensorSuiteNode):
|
||||
sensor_suites[sensor_node.ip_address] = sensor_node
|
||||
if sensor_node.provides_pnt:
|
||||
sensor_classes["PNT"].append(sensor_node.ip_address)
|
||||
if sensor_node.provides_accel:
|
||||
sensor_classes["ACCEL"].append(sensor_node.ip_address)
|
||||
if sensor_node.provides_gyro:
|
||||
sensor_classes["GYRO"].append(sensor_node.ip_address)
|
||||
if sensor_node.provides_mag:
|
||||
sensor_classes["MAG"].append(sensor_node.ip_address)
|
||||
sensor_classes[sensor_node.sensor_class].append(sensor_node.ip_address)
|
||||
|
||||
@@ -2,7 +2,6 @@
|
||||
|
||||
[ext_resource type="Script" uid="uid://d264hr5vl2kqv" path="res://vehicle/scripts/vehicle_node.gd" id="1_2o63x"]
|
||||
[ext_resource type="Script" uid="uid://bflafrhk4eq4w" path="res://sensors/sensor_spawner.gd" id="2_2fi1b"]
|
||||
[ext_resource type="PackedScene" uid="uid://trl3gugdy3hw" path="res://sensors/sensor_suite.tscn" id="3_1ktiy"]
|
||||
|
||||
[node name="Vehicle" type="Node3D" unique_id=1694630995]
|
||||
script = ExtResource("1_2o63x")
|
||||
@@ -10,5 +9,4 @@ metadata/_custom_type_script = "uid://d264hr5vl2kqv"
|
||||
|
||||
[node name="SensorSpawner" type="Node3D" parent="." unique_id=1875871474]
|
||||
script = ExtResource("2_2fi1b")
|
||||
sensor_scene = ExtResource("3_1ktiy")
|
||||
metadata/_custom_type_script = "uid://bflafrhk4eq4w"
|
||||
|
||||
Reference in New Issue
Block a user