diff --git a/bisect.gd b/bisect.gd index 2113fd4..c10e949 100644 --- a/bisect.gd +++ b/bisect.gd @@ -42,3 +42,45 @@ func bisect(a: Array, x, lo: int = 0, hi=null): func insort(a: Array, x, lo: int = 0, hi=null): return insort_right(a, x, lo, hi) + +func reverse_insort_right(a: Array, x, lo: int = 0, hi=null): + lo = reverse_bisect_right(a, x, lo, hi) + a.insert(lo, x) + +func reverse_bisect_right(a: Array, x, lo: int = 0, hi=null): + if lo < 0: + return + if hi == null: + hi = a.size() + var mid: int = 0 + while lo < hi: + mid = (lo + hi) / 2 + if -x < -a[mid]: + hi = mid + else: + lo = mid + 1 + return lo + +func reverse_insort_left(a: Array, x, lo:int = 0, hi=null): + lo = reverse_bisect_left(a, x, lo, hi) + a.insert(lo, x) + +func reverse_bisect_left(a: Array, x, lo:int = 0, hi=null): + if lo < 0: + return + if hi == null: + hi = a.size() + var mid: int = 0 + while lo < hi: + mid = (lo + hi) / 2 + if -a[mid] < -x: + lo = mid + 1 + else: + hi = mid + return lo + +func reverse_bisect(a: Array, x, lo: int = 0, hi=null): + return reverse_bisect_right(a, x, lo, hi) + +func reverse_insort(a: Array, x, lo: int = 0, hi=null): + return reverse_insort_right(a, x, lo, hi) diff --git a/networking/scripts/udp_broadcast.gd b/networking/scripts/udp_broadcast.gd index 621c0f1..3790cc1 100644 --- a/networking/scripts/udp_broadcast.gd +++ b/networking/scripts/udp_broadcast.gd @@ -5,7 +5,7 @@ extends Node var udp = PacketPeerUDP.new() var connected = false -var fast_broadcast_rate = 1.0 +var fast_broadcast_rate = 0.5 var slow_broadcast_rate = 10.0 var current_broadcast_rate = fast_broadcast_rate var last_broadcast = 0 @@ -16,6 +16,7 @@ var thread: Thread var mutex: Mutex var signal_queue = [] var exit_thread = false +var use_thread = true func _ready(): _try_set_ip() @@ -23,44 +24,54 @@ func _ready(): udp.set_dest_address("192.168.0.255", 5000) SensorSignals.new_sensor_connected.connect(_on_sensor_connected) - thread = Thread.new() + use_thread = OS.get_processor_count() > 1 mutex = Mutex.new() - - thread.start(_thread_function) + if use_thread: + thread = Thread.new() + print('Starting UDP Broadcast thread') + thread.start(_thread_function) + else: + print('UDP server running in single threaded mode') func _process(delta: float) -> void: mutex.lock() last_broadcast += delta mutex.unlock() + if not use_thread: + _process_broadcast() func _exit_tree() -> void: - mutex.lock() - exit_thread = true - mutex.unlock() - thread.wait_to_finish() + if use_thread: + mutex.lock() + exit_thread = true + mutex.unlock() + thread.wait_to_finish() func _thread_function(): + print('UDP Broadcast thread started') while true: mutex.lock() var should_exit = exit_thread mutex.unlock() if should_exit: break - - if ip_address.length() == 0: - _try_set_ip() - return mutex.lock() var do_broadcast = last_broadcast >= current_broadcast_rate mutex.unlock() if do_broadcast: - # Try to contact server mutex.lock() last_broadcast = 0 mutex.unlock() - var broadcast_str = ip_address + "|%s" % Time.get_datetime_string_from_system(true) - print('Broadcasting to UDP: ', broadcast_str) - udp.put_packet(broadcast_str.to_utf8_buffer()) + _process_broadcast() + +func _process_broadcast(): + if ip_address.length() == 0: + _try_set_ip() + return + # Try to contact server + var broadcast_str = ip_address + "|%s" % Time.get_datetime_string_from_system(true) + print('Broadcasting to UDP: ', broadcast_str) + udp.put_packet(broadcast_str.to_utf8_buffer()) func _try_set_ip(): for address in IP.get_local_addresses(): diff --git a/networking/scripts/udp_server.gd b/networking/scripts/udp_server.gd index d44c886..9844e67 100644 --- a/networking/scripts/udp_server.gd +++ b/networking/scripts/udp_server.gd @@ -12,9 +12,10 @@ var awaiting_sensors = {} var invalid_sensors = {} var valid_sensors = {} +var use_thread: bool = false var thread: Thread var mutex: Mutex -var exit_thread = false +var exit_thread = true var signal_queue = [] @@ -27,10 +28,16 @@ func _ready(): print('Listening on port: ', port) mutex = Mutex.new() - thread = Thread.new() - thread.start(_thread_function) + if use_thread: + thread = Thread.new() + print('Starting UDP Server thread') + thread.start(_thread_function) + else: + print('UDP server running in single threaded mode') func _process(delta: float) -> void: + if not use_thread: + _process_connections() while signal_queue.size() > 0: mutex.lock() var signal_data = signal_queue.pop_front() @@ -51,10 +58,11 @@ func _process(delta: float) -> void: DataSignals.magnetometer_data_received.emit(signal_data[1], signal_data[2], signal_data[3]) func _exit_tree() -> void: - mutex.lock() - exit_thread = true - mutex.unlock() - thread.wait_to_finish() + if use_thread: + mutex.lock() + exit_thread = true + mutex.unlock() + thread.wait_to_finish() func _on_sensor_approved(sensor_ip: String): mutex.lock() @@ -72,6 +80,7 @@ func _on_sensor_declined(sensor_ip: String): mutex.unlock() func _thread_function(): + print('UDP Server thread started') while true: mutex.lock() var should_exit = exit_thread @@ -177,15 +186,13 @@ func _handle_new_peer_request(peer_ip: String, packet: PackedByteArray): 10 | 1 byte | bool | peer provides quaternion orientation """ mutex.lock() - awaiting_sensors[peer_ip] = [ - #packet.decode_u8(4), - packet.decode_u8(5), - packet.decode_u8(6), - packet.decode_u8(7), - packet.decode_u8(8), - #packet.decode_u8(9), - #packet.decode_u8(10), - ] + print(packet) + print(packet.get(4)) + print(0xf1) + if packet.get(4) == 0xf1: + awaiting_sensors[peer_ip] = "VEHICLE" + elif packet.get(4) == 0x23: + awaiting_sensors[peer_ip] = "LIDAR_2D" signal_queue.append(["sensor_request_received", peer_ip, awaiting_sensors[peer_ip]]) mutex.unlock() diff --git a/project.godot b/project.godot index f684a19..2f1fd67 100644 --- a/project.godot +++ b/project.godot @@ -28,6 +28,10 @@ DisplayData="*uid://xunxbytiofa2" Bisect="*uid://crwgaltrjx4ff" TimeHelpers="*uid://dqtc81pc74lcr" +[debug] + +file_logging/enable_file_logging=true + [display] window/size/viewport_width=800 diff --git a/sensor_approval/SensorApprovalController.gd b/sensor_approval/SensorApprovalController.gd index 4db07ea..5b60a82 100644 --- a/sensor_approval/SensorApprovalController.gd +++ b/sensor_approval/SensorApprovalController.gd @@ -32,9 +32,9 @@ func _on_settings_closed(): add_child(sensor_approval_nodes[val]) _update_container_position() -func _on_sensor_request_received(sensor_ip: String, data_provided: Array): +func _on_sensor_request_received(sensor_ip: String, sensor_class: String): sensor_approval_nodes[sensor_ip] = sensor_approval_scene.instantiate() - sensor_approval_nodes[sensor_ip].set_provided_data(sensor_ip, data_provided) + sensor_approval_nodes[sensor_ip].set_provided_data(sensor_ip, sensor_class) if not settings_open: add_child(sensor_approval_nodes[sensor_ip]) _update_container_position() diff --git a/sensor_approval/provided_data.gd b/sensor_approval/provided_data.gd index 1f1e7d1..730aff9 100644 --- a/sensor_approval/provided_data.gd +++ b/sensor_approval/provided_data.gd @@ -11,9 +11,10 @@ offset | size | type | description 9 | 1 byte | bool | peer provides euler orientation 10 | 1 byte | bool | peer provides quaternion orientation """ -var data_labels = ["Date Time", "PNT", "Accel", "Gyro", "Magnet", "Euler", "Quat"] +var data_labels = ["Date Time", "PNT", "Accel", "Gyro", "Magnet", "Euler", "Quat", "2D Lidar"] -var provided_data: Array = [0, 0, 0, 0, 0, 0, 0] +var sensor_class: String = '' +var provided_data: Array = [0, 0, 0, 0, 0, 0, 0, 0] var data_columns: Array @export var data_column_scene: PackedScene @@ -35,5 +36,9 @@ func _setup_data_columns(): func _process(delta: float) -> void: pass -func set_provided_data(incoming_provided_data): - provided_data = incoming_provided_data +func set_provided_data(incoming_sensor_class: String): + sensor_class = incoming_sensor_class.capitalize() + if incoming_sensor_class == "VEHICLE": + provided_data = [0, 1, 1, 1, 1, 0, 0, 0] + elif incoming_sensor_class == "LIDAR_2D": + provided_data = [0, 1, 1, 1, 0, 0, 0, 1] diff --git a/sensor_approval/sensor_approval.gd b/sensor_approval/sensor_approval.gd index df23497..ecf485f 100644 --- a/sensor_approval/sensor_approval.gd +++ b/sensor_approval/sensor_approval.gd @@ -5,7 +5,7 @@ var ip_label: Label var data_instance: Node var sensor_ip: String = "127.0.0.1" -var data_labels = ["PNT", "Accel", "Gyro", "Magnet"] +var data_labels = ["PNT", "Accel", "Gyro", "Magnet", "2D Lidar"] var provided_data: Array = [0, 0, 0, 0] var data_columns: Array @@ -27,11 +27,14 @@ func _setup_data_columns(): column_instance.get_node('DataValue').text = "True" container.add_child(column_instance) -func set_provided_data(incoming_sensor_ip: String, incoming_provided_data: Array): +func set_provided_data(incoming_sensor_ip: String, sensor_class: String): sensor_ip = incoming_sensor_ip ip_label = get_node('HBoxContainer/Label') ip_label.text = sensor_ip - provided_data = incoming_provided_data + if sensor_class == "VEHICLE": + provided_data = [1, 1, 1, 1, 0] + elif sensor_class == "LIDAR_2D": + provided_data = [1, 1, 1, 0, 1] if data_columns.size() == 0: _setup_data_columns() else: diff --git a/sensors/lidar_2d_sensor_node.gd b/sensors/lidar_2d_sensor_node.gd new file mode 100644 index 0000000..15135a5 --- /dev/null +++ b/sensors/lidar_2d_sensor_node.gd @@ -0,0 +1,14 @@ +extends BaseSensorNode +class_name Lidar2DSensorNode + +# Called when the node enters the scene tree for the first time. +func _ready() -> void: + pass # Replace with function body. + + +# Called every frame. 'delta' is the elapsed time since the previous frame. +func _process(delta: float) -> void: + pass + +func _get_sensor_class(): + return "LIDAR_2D" diff --git a/sensors/lidar_2d_sensor_node.gd.uid b/sensors/lidar_2d_sensor_node.gd.uid new file mode 100644 index 0000000..b346419 --- /dev/null +++ b/sensors/lidar_2d_sensor_node.gd.uid @@ -0,0 +1 @@ +uid://c00jgww14wp8w diff --git a/sensors/sensor_spawner.gd b/sensors/sensor_spawner.gd index 283b434..e54911d 100644 --- a/sensors/sensor_spawner.gd +++ b/sensors/sensor_spawner.gd @@ -8,8 +8,14 @@ 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_node = VehicleSensorNode.new() +func _on_new_sensor_connected(sensor_ip: String, sensor_type: String) -> void: + var sensor_node: BaseSensorNode + if sensor_type == "VEHICLE": + sensor_node = VehicleSensorNode.new() + elif sensor_type == "LIDAR_2D": + sensor_node = Lidar2DSensorNode.new() + else: + return sensor_node.ip_address = sensor_ip add_child(sensor_node) vehicle.add_sensor_node(sensor_node) diff --git a/sensors/vehicle_sensor_node.gd b/sensors/vehicle_sensor_node.gd index 605f446..908117b 100644 --- a/sensors/vehicle_sensor_node.gd +++ b/sensors/vehicle_sensor_node.gd @@ -16,10 +16,12 @@ var gps_timestamps = [] var coordinates = [] var gps_positions = [] var altitudes = [] +var alt_coefficients = [] var altitude_m: get: return _get_current_altitude() var speed_data = [] +var speed_coefficients = [] var speed_kmh: get: return _get_current_speed() @@ -32,6 +34,12 @@ var pitch_data = [] var pitch_rad: float var roll_data = [] var roll_rad: float +var lateral_g_data = [] +var x_coefficients = [] +var y_coefficients = [] +var lateral_accel: + get: + return _get_current_gs() var mag_timestamps = [] var mag_vecs = [] @@ -57,7 +65,7 @@ func _on_gps_data_received(source_ip: String, timestamp_list: Array, new_coordin 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 insert_idx = Bisect.reverse_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]) @@ -69,11 +77,13 @@ func _on_gps_data_received(source_ip: String, timestamp_list: Array, new_coordin 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() + gps_timestamps.pop_back() + coordinates.pop_back() + gps_positions.pop_back() + altitudes.pop_back() + speed_data.pop_back() + _update_alt_coefficients() + _update_speed_coefficients() func _on_sat_data_received(source_ip: String, timestamp: float): if source_ip != ip_address: @@ -85,7 +95,7 @@ func _on_accel_data_received(source_ip: String, timestamp_list: Array, accel: Ve 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 insert_idx = Bisect.reverse_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) @@ -95,14 +105,18 @@ func _on_accel_data_received(source_ip: String, timestamp_list: Array, accel: Ve g_vecs.insert(insert_idx, g_accel) pitch_data.insert(insert_idx, pitch) roll_data.insert(insert_idx, roll) + var lateral_g = Vector2(linear_accel.x, linear_accel.y) + lateral_g_data.insert(insert_idx, lateral_g.normalized()*lateral_g.length()) 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() + accel_timestamps.pop_back() + linear_vecs.pop_back() + g_vecs.pop_back() + pitch_data.pop_back() + roll_data.pop_back() + lateral_g_data.pop_back() _update_avg_pitch() _update_avg_roll() + _update_g_coefficients() func _on_mag_data_received(source_ip: String, timestamp_list: Array, mag_vec: Vector3): if source_ip != ip_address: @@ -110,64 +124,106 @@ func _on_mag_data_received(source_ip: String, timestamp_list: Array, mag_vec: Ve 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 insert_idx = Bisect.reverse_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) + if new_heading_rad < 0: + new_heading_rad += 2*PI 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() + mag_timestamps.pop_back() + mag_vecs.pop_back() + heading_data.pop_back() func _on_gyro_data_received(source_ip: String, timestamp: float): if source_ip != ip_address: return +func get_derrivative(dataset: Array) -> Array: + if dataset.size() < 2: + return [0, []] + 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] + +func get_array_avg(dataset: Array): + var data_sum = 0 + for i in dataset: + data_sum += i + return data_sum / dataset.size() + +func get_coefficients(dataset: Array, order: int): + if order == 0: + return [] + if dataset.size() <= order: + order = dataset.size() - 1 + var coefficients = [] + var tmp_dataset = dataset + var dx_result: Array + for i in range(order): + dx_result = get_derrivative(tmp_dataset) + coefficients.append(dx_result[0]) + tmp_dataset = dx_result[1] + return coefficients + +func _update_alt_coefficients(): + if gps_timestamps.size() < 2: + return + alt_coefficients = get_coefficients(altitudes, 2) + 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) + var time_diff = max(0, TimeHelpers.calc_time_diff_24h(gps_timestamps[0], current_timestamp)) + var current_value = altitudes[0] + for idx in range(alt_coefficients.size()): + current_value += alt_coefficients[idx] * (time_diff ** idx+1) + return current_value + +func _update_speed_coefficients(): + if gps_timestamps.size() < 2: + return + speed_coefficients = get_coefficients(speed_data, 2) func _get_current_speed(): if gps_timestamps.size() < 3: - return null - var speed_dx = [] - var speed_dx_sum = 0 + return 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) + var current_value = speed_data[0] + for idx in range(speed_coefficients.size()): + current_value += speed_coefficients[idx] * (time_diff ** idx+1) + return current_value + +func _update_g_coefficients(): + if accel_timestamps.size() < 2: + return + var x_dataset = [] + var y_dataset = [] + for i in lateral_g_data: + x_dataset.append(i.x) + y_dataset.append(i.y) + x_coefficients = get_coefficients(x_dataset, 3) + y_coefficients = get_coefficients(y_dataset, 3) + +func _get_current_gs(): + if accel_timestamps.size() < 3: + return + var current_timestamp = TimeHelpers.get_current_sensor_time() + var time_diff = max(0, TimeHelpers.calc_time_diff_24h(accel_timestamps[0], current_timestamp)) + var current_x = lateral_g_data[0].x + var current_y = lateral_g_data[0].y + for idx in range(speed_coefficients.size()): + current_x += x_coefficients[idx] * (time_diff ** idx+1) + current_y += y_coefficients[idx] * (time_diff ** idx+1) + return Vector2(current_x, current_y) func _update_avg_pitch(): var pitch_sum = 0 @@ -183,7 +239,7 @@ func _update_avg_roll(): func _update_avg_heading(): var heading_sum = 0 - for i in range(mag_timestamps.size()): + for i in range(min(mag_timestamps.size(), 2)): heading_sum += heading_data[i] heading_rad = heading_sum / heading_data.size() diff --git a/signals/server_signals.gd b/signals/server_signals.gd index 005a0b3..0c80917 100644 --- a/signals/server_signals.gd +++ b/signals/server_signals.gd @@ -1,5 +1,5 @@ extends Node -signal sensor_request_received(sensor_ip_address: String, data_provided: Array) +signal sensor_request_received(sensor_ip_address: String, sensor_class: String) signal sensor_approved(sensor_ip_adress: String) signal sensor_declined(sensor_ip_address: String) diff --git a/vehicle/scripts/vehicle_node.gd b/vehicle/scripts/vehicle_node.gd index c4f296e..d7a1712 100644 --- a/vehicle/scripts/vehicle_node.gd +++ b/vehicle/scripts/vehicle_node.gd @@ -16,6 +16,7 @@ var host_id: String = '' var sensor_nodes = {} var sensor_classes = { "VEHICLE": [], + "LIDAR_2D": [], } var timer: float = 0. @@ -37,6 +38,10 @@ var pitch_rad: float: get: return _get_pitch_rad() +var lateral_accel: Vector2: + get: + return _get_lateral_accel() + var heading_rad: float: get: return _get_heading_rad() @@ -124,6 +129,19 @@ func _get_pitch_rad(): return 0 return pitch_sum / pitch_count +func _get_lateral_accel(): + var gs_sum: Vector2 = Vector2.ZERO + var gs_count: int = 0 + for sensor_ip in sensor_classes["VEHICLE"]: + var sensor_gs = sensor_nodes[sensor_ip].lateral_accel + if sensor_gs == null: + continue + gs_sum += sensor_gs + gs_count += 1 + if gs_count == 0: + return Vector2.ZERO + return gs_sum / gs_count + func _get_heading_rad(): var heading_sum: float = 0 var heading_count: int = 0 diff --git a/views/2d/View2D.tscn b/views/2d/View2D.tscn index 6848e46..e4e53d8 100644 --- a/views/2d/View2D.tscn +++ b/views/2d/View2D.tscn @@ -15,7 +15,7 @@ script = ExtResource("1_n6fjn") [node name="HUD" type="CanvasLayer" parent="." unique_id=1826106128] [node name="Compass" parent="HUD" unique_id=2062816665 instance=ExtResource("1_fyqef")] -position = Vector2(114.99999, 538.00006) +position = Vector2(104, 486) [node name="TotalSatCount" parent="HUD" unique_id=931018379 instance=ExtResource("2_3b2b5")] visible = false @@ -64,7 +64,7 @@ scale = Vector2(0.05, 0.05) sat_category = "QZSS" [node name="AltitudeWidget" parent="HUD" unique_id=1004159593 instance=ExtResource("3_g6l6n")] -position = Vector2(243, 494) +position = Vector2(170, 481) [node name="AccelerometerWidget" parent="HUD" unique_id=1057256615 instance=ExtResource("4_qui0h")] position = Vector2(675, 400) diff --git a/views/ViewController.gd b/views/ViewController.gd index 85d3c8a..b414b71 100644 --- a/views/ViewController.gd +++ b/views/ViewController.gd @@ -25,7 +25,7 @@ func _ready() -> void: func add_widget(widget_type: String, widget_position: Vector2, widget_scale: float): pass -func _on_sensor_request_received(sensor_ip, sensor_data): +func _on_sensor_request_received(sensor_ip, sensor_class): _on_open_settings_pressed() func _on_sensor_approced(sensor_ip): diff --git a/widgets/accelerometer/scripts/accelerometer_widget_node.gd b/widgets/accelerometer/scripts/accelerometer_widget_node.gd index fb488bd..ee31a52 100644 --- a/widgets/accelerometer/scripts/accelerometer_widget_node.gd +++ b/widgets/accelerometer/scripts/accelerometer_widget_node.gd @@ -11,7 +11,6 @@ var origin: Vector2 = Vector2(0., 0.) # Called when the node enters the scene tree for the first time. func _ready() -> void: super._ready() - Signals.linear_acceleration_updated.connect(_on_linear_acceleration_updated) label = get_node('Label') accel_vector = get_node("AccelVector") accel_vector.points = [origin, origin] @@ -20,13 +19,13 @@ func _ready() -> void: y_vector = get_node("YVector") y_vector.points = [origin, origin] -func _on_linear_acceleration_updated(sensor_ip: String, linear_acceleration: Array): - var accel_vec = Vector2(-linear_acceleration[1], linear_acceleration[0]) - var vec_norm: Vector2 = accel_vec.normalized() - var vec_length: float = accel_vec.length() - var accel_g = vec_length / Constants.g - var x_vec = Vector2(vec_norm.x*vec_length, 0) - accel_vector.points = [origin, accel_vec * 10] - x_vector.points = [origin, x_vec * 10] - y_vector.points = [x_vec * 10, accel_vec * 10] - label.text = str(round(accel_g*100)/100) + ' g' +func _update_data(): + if y_vector == null: + return + var lateral_gs = host_vehicle.lateral_accel + var accel_g = lateral_gs.length() / Constants.g + var x_vec = Vector2(lateral_gs.x, 0) + accel_vector.points = [origin, lateral_gs * 4] + x_vector.points = [origin, x_vec * 4] + y_vector.points = [x_vec * 4, lateral_gs * 4] + label.text = str(round(accel_g*10)/10) + ' g' diff --git a/widgets/altitude/altitude_widget.tscn b/widgets/altitude/altitude_widget.tscn index 00eb41a..d6b5261 100644 --- a/widgets/altitude/altitude_widget.tscn +++ b/widgets/altitude/altitude_widget.tscn @@ -8,22 +8,22 @@ script = ExtResource("1_hmdxp") metadata/_custom_type_script = "uid://b3rfje1auw6to" [node name="Panel" type="Panel" parent="." unique_id=1393400967] -offset_right = 306.0 -offset_bottom = 70.0 +offset_right = 460.0 +offset_bottom = 120.0 [node name="Label" type="Label" parent="." unique_id=1070194714] -offset_right = 306.0 -offset_bottom = 70.0 -theme_override_font_sizes/font_size = 32 -text = "Altitude: 99999 ft" +offset_right = 460.0 +offset_bottom = 120.0 +theme_override_font_sizes/font_size = 80 +text = "Alt: 99999 ft" horizontal_alignment = 1 vertical_alignment = 1 [node name="MovableControl" type="Control" parent="." unique_id=1186292353 node_paths=PackedStringArray("move_target")] layout_mode = 3 anchors_preset = 0 -offset_right = 306.0 -offset_bottom = 70.0 +offset_right = 460.0 +offset_bottom = 120.0 mouse_filter = 1 script = ExtResource("2_34sps") move_target = NodePath("..") diff --git a/widgets/altitude/scripts/altitude_widget_node.gd b/widgets/altitude/scripts/altitude_widget_node.gd index 3ce441f..82b84d2 100644 --- a/widgets/altitude/scripts/altitude_widget_node.gd +++ b/widgets/altitude/scripts/altitude_widget_node.gd @@ -14,4 +14,4 @@ func _ready() -> void: func _update_data(): if label_node == null: return - label_node.text = 'Altitude: ' + str(int(round(host_vehicle.altitude_m * Constants.M2FT))) + ' ft' + label_node.text = 'Alt: ' + str(int(round(host_vehicle.altitude_m * Constants.M2FT))) + ' ft' diff --git a/widgets/compass/compass_widget_scene.tscn b/widgets/compass/compass_widget_scene.tscn index ed619d9..2aabe03 100644 --- a/widgets/compass/compass_widget_scene.tscn +++ b/widgets/compass/compass_widget_scene.tscn @@ -14,14 +14,20 @@ scale = Vector2(0.1, 0.1) texture = ExtResource("2_qf1yp") [node name="Label" type="Label" parent="." unique_id=338110309] +anchors_preset = 7 +anchor_left = 0.5 +anchor_top = 1.0 +anchor_right = 0.5 +anchor_bottom = 1.0 offset_left = -50.0 -offset_top = -350.0 +offset_top = -300.0 offset_right = 50.0 -offset_bottom = -150.0 +grow_horizontal = 2 +grow_vertical = 0 +pivot_offset = Vector2(50, 300) theme_override_font_sizes/font_size = 80 text = "N" horizontal_alignment = 1 -vertical_alignment = 1 justification_flags = 161 [node name="MovableControl" type="Control" parent="." unique_id=124696843 node_paths=PackedStringArray("move_target")] diff --git a/widgets/compass/scripts/compass_widget_node.gd b/widgets/compass/scripts/compass_widget_node.gd index 493f9e7..05d9620 100644 --- a/widgets/compass/scripts/compass_widget_node.gd +++ b/widgets/compass/scripts/compass_widget_node.gd @@ -5,6 +5,7 @@ class_name CompassWidgetNode @export_range(0., 359., 0.1) var north_deg = 0. var north_rad: float var indicator: Sprite2D +var label: Label # Called when the node enters the scene tree for the first time. func _ready() -> void: @@ -13,8 +14,31 @@ func _ready() -> void: north_deg = 0. north_rad = deg_to_rad(north_deg) indicator = get_node("CompassIndicator") + label = get_node("Label") func _update_data(): - if indicator == null: + if label == null: return - indicator.rotation = host_vehicle.heading_rad + #label.rotation = -host_vehicle.heading_rad + label.text = get_direction_label(host_vehicle.heading_rad) + +func get_direction_label(heading_rad): + var heading_deg = rad_to_deg(heading_rad) + if 0 <= heading_deg and heading_deg <= 22.5: + return 'N' + elif 22.5 < heading_deg and heading_deg <= 67.5: + return "NE" + elif 67.5 < heading_deg and heading_deg <= 112.5: + return "E" + elif 112.5 < heading_deg and heading_deg <= 157.5: + return "SE" + elif 157.5 < heading_deg and heading_deg <= 202.5: + return "S" + elif 202.5 < heading_deg and heading_deg <= 247.5: + return "SW" + elif 247.5 < heading_deg and heading_deg <= 292.5: + return "W" + elif 292.5 < heading_deg and heading_deg <= 337.5: + return "NW" + else: + return "N" diff --git a/widgets/speed/speed_widget.tscn b/widgets/speed/speed_widget.tscn index cffd42a..5b2914b 100644 --- a/widgets/speed/speed_widget.tscn +++ b/widgets/speed/speed_widget.tscn @@ -14,14 +14,14 @@ offset_bottom = 40.0 [node name="SpeedLabel" type="Label" parent="VBoxContainer" unique_id=2101712075] custom_minimum_size = Vector2(150, 0) layout_mode = 2 -theme_override_font_sizes/font_size = 48 +theme_override_font_sizes/font_size = 80 text = "10" horizontal_alignment = 1 [node name="UnitLabel" type="Label" parent="VBoxContainer" unique_id=123505815] custom_minimum_size = Vector2(100, 0) layout_mode = 2 -theme_override_font_sizes/font_size = 16 +theme_override_font_sizes/font_size = 26 text = "MPH" horizontal_alignment = 1