191 lines
6.4 KiB
GDScript
191 lines
6.4 KiB
GDScript
|
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()
|
||
|
|
|
||
|
|
|