57 lines
1.9 KiB
GDScript
57 lines
1.9 KiB
GDScript
|
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()
|