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

Bunch of changes

Browse Source
This commit is contained in:
smithserver_admin
2026-08-01 18:19:21 -06:00
parent 04f3e2ba07
commit 7102175711
9 changed files with 184 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sensors/vehicle_sensor_node.gd
+42 -13
View File
@@ -49,20 +49,52 @@ var heading_rad: float
func _ready() -> void:
super._ready()
DataSignals.gps_data_received.connect(_on_gps_data_received)
DataSignals.raw_data_received.connect(_on_raw_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]
func _on_raw_data_received(source_ip: String, sensor_type: String, timestamp_list: Array, data: Array):
if source_ip != ip_address:
return
if sensor_type == "GPS":
var parsed_coordinates = [
(float(data[0]) / 10.**9) - 90,
(float(data[1]) / 10.**9) - 180,
(float(data[2]) / 1000.),
(float(data[3]) / 100.),
]
var speed = float(data[4]) / 1000000.
var dop = [
float(data[5]) / 1000.,
float(data[6]) / 1000.,
float(data[7]) / 1000.,
]
_on_gps_data_received(timestamp_list, parsed_coordinates, speed, dop)
if sensor_type == "ACCEL":
var accel = Vector3(
float(data[0]) / 1000000.,
float(data[1]) / 1000000.,
float(data[2]) / 1000000.
)
var linear_accel = Vector3(
float(data[3]) / 1000000.,
float(data[4]) / 1000000.,
float(data[5]) / 1000000.
)
var g_accel = Vector3(
float(data[6]) / 1000000.,
float(data[7]) / 1000000.,
float(data[8]) / 1000000.
)
_on_accel_data_received(timestamp_list, accel, linear_accel, g_accel)
func _on_gps_data_received(timestamp_list: Array, new_coordinates: Array, speed: float, dop: Array):
# coordinates = [lat rad, lon rad, alt m, geoid diff m]
# dop = [pdop, hdop, vdop]
var timestamp: float = 3600.*timestamp_list[0] + 60.*timestamp_list[1] + timestamp_list[2] + timestamp_list[3]
if gps_timestamps.size() > 0 and (timestamp - gps_timestamps[-1]) < -1:
gps_timestamps.clear()
@@ -73,7 +105,7 @@ func _on_gps_data_received(source_ip: String, timestamp_list: Array, new_coordin
if gps_timestamps.size() == max_data_length and not TimeHelpers.is_timestamp_newer_24h(gps_timestamps[-1], timestamp):
return
var insert_idx = Bisect.reverse_bisect(gps_timestamps, timestamp)
var altitude_m = new_coordinates[2] + new_coordinates[3]
var altitude = 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])
@@ -81,8 +113,8 @@ func _on_gps_data_received(source_ip: String, timestamp_list: Array, new_coordin
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)
altitudes.insert(insert_idx, altitude)
speed_data.insert(insert_idx, speed)
if gps_timestamps.size() > max_data_length:
gps_timestamps.pop_back()
coordinates.pop_back()
@@ -96,9 +128,7 @@ 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
func _on_accel_data_received(timestamp_list: Array, accel: Vector3, linear_accel: Vector3, g_accel: Vector3):
var timestamp: float = 3600.*timestamp_list[0] + 60.*timestamp_list[1] + timestamp_list[2] + timestamp_list[3]
if accel_timestamps.size() > 0 and (timestamp - accel_timestamps[-1]) < -1:
accel_timestamps.clear()
@@ -166,9 +196,8 @@ func get_derrivative(dataset: Array) -> Array:
var deltas = []
var delta_sum = 0
for i in range(dataset.size()-1):
var delta = dataset[i+1] - dataset[i]
deltas.append(dataset[i+1] - dataset[i])
return [delta_sum / deltas.size(), deltas]
return [float(delta_sum) / deltas.size(), deltas]
func get_array_avg(dataset: Array):
var data_sum = 0