diff --git a/networking/scripts/udp_server.gd b/networking/scripts/udp_server.gd index 9844e67..fc6da97 100644 --- a/networking/scripts/udp_server.gd +++ b/networking/scripts/udp_server.gd @@ -56,6 +56,8 @@ func _process(delta: float) -> void: DataSignals.accelerometer_data_received.emit(signal_data[1], signal_data[2], signal_data[3], signal_data[4], signal_data[5]) elif signal_data[0] == "magnetometer_data_received": DataSignals.magnetometer_data_received.emit(signal_data[1], signal_data[2], signal_data[3]) + elif signal_data[0] == "lidar_2d_data_received": + DataSignals.lidar_2d_data_received.emit(signal_data[1], signal_data[2], signal_data[3], signal_data[4]) func _exit_tree() -> void: if use_thread: @@ -167,6 +169,8 @@ func _handle_six_fps_message(peer: PacketPeerUDP, packet: PackedByteArray): elif message_class == 0x2a and message_id == 0x39: #print('Mag packet received') _handle_magnetic_packet(peer_ip, packet) + elif message_class == 0x2a and message_id == 0x6f: + _handle_lidar_2d_packet(peer_ip, packet) elif message_class == 0x2a and message_id == 0xa7: _handle_public_key_packet(peer_ip, packet) elif message_class == 0x2a and message_id == 0x5e: # not in use @@ -186,9 +190,6 @@ func _handle_new_peer_request(peer_ip: String, packet: PackedByteArray): 10 | 1 byte | bool | peer provides quaternion orientation """ mutex.lock() - print(packet) - print(packet.get(4)) - print(0xf1) if packet.get(4) == 0xf1: awaiting_sensors[peer_ip] = "VEHICLE" elif packet.get(4) == 0x23: @@ -238,6 +239,7 @@ func _handle_position_packet(peer_ip: String, packet: PackedByteArray): var vdop: float = packet.decode_u32(49) / 1000.0 mutex.lock() signal_queue.append(["gps_data_received", peer_ip, timestamp_list, [lat, lon, alt_m, goid_m], speed_kmh, [pdop, hdop, vdop]]) + signal_queue.append(["raw_data_received", peer_ip, "GPS", timestamp_list, {"lat": lat, "lon": lon, "alt_m": alt_m, "speed_kmh": speed_kmh, "pdop": pdop, "hdop": hdop, "vdop": vdop}]) mutex.unlock() func _handle_gps_sats_packet(peer_ip: String, packet: PackedByteArray): @@ -311,7 +313,22 @@ func _handle_magnetic_packet(peer_ip: String, packet: PackedByteArray): mutex.lock() signal_queue.append(["magnetometer_data_received", peer_ip, timestamp_list, Vector3(x, y, z)]) mutex.unlock() - + +func _handle_lidar_2d_packet(peer_ip: String, packet: PackedByteArray): + var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11)) + mutex.lock() + signal_queue.append(["on_time_data_received", timestamp_list]) + mutex.unlock() + var point_count = packet.decode_u8(11) + var point_data = [] + for idx in range(point_count): + var offset = idx * 7 + var angle: float = float(packet.decode_u32(12 + offset)) / 10000 + var distance: float = float(packet.decode_u16(12 + offset + 4)) / 1000 + var intensity: int = packet.decode_u8(12 + offset + 6) + point_data.append([angle, distance, intensity]) + signal_queue.append(["lidar_2d_data_received", peer_ip, timestamp_list, point_count, point_data]) + func _handle_public_key_packet(peer_ip: String, packet: PackedByteArray): var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11)) mutex.lock() diff --git a/project.godot b/project.godot index 2f1fd67..4cfb994 100644 --- a/project.godot +++ b/project.godot @@ -36,6 +36,7 @@ file_logging/enable_file_logging=true window/size/viewport_width=800 window/size/viewport_height=800 +window/size/mode=4 [physics] @@ -44,3 +45,4 @@ window/size/viewport_height=800 [rendering] rendering_device/driver.windows="d3d12" +renderer/rendering_method="mobile" diff --git a/sensors/lidar_2d_sensor_node.gd b/sensors/lidar_2d_sensor_node.gd index 15135a5..af4781e 100644 --- a/sensors/lidar_2d_sensor_node.gd +++ b/sensors/lidar_2d_sensor_node.gd @@ -1,9 +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. + super._ready() + DataSignals.lidar_2d_data_received.connect(_on_lidar_data_received) + DataSignals.gps_data_received.connect(_on_gps_data_received) + DataSignals.accelerometer_data_received.connect(_on_accel_data_received) + DataSignals.gyroscope_data_received.connect(_on_gyro_data_received) # Called every frame. 'delta' is the elapsed time since the previous frame. @@ -12,3 +17,14 @@ func _process(delta: float) -> void: func _get_sensor_class(): return "LIDAR_2D" + +func _on_lidar_data_received(sensor_ip: String, timestamp_list: Array, lidar_point_count: int, lidar_data: Array):pass + +func _on_gps_data_received(source_ip: String, timestamp_list: Array, new_coordinates: Array, speed_kmh: float, dop: Array): + pass + +func _on_accel_data_received(source_ip: String, timestamp_list: Array, accel: Vector3, linear_accel: Vector3, g_accel: Vector3): + pass + +func _on_gyro_data_received(source_ip: String, timestamp_list: Array, gyro_data: Array): + pass diff --git a/sensors/vehicle_sensor_node.gd b/sensors/vehicle_sensor_node.gd index 908117b..280dcbd 100644 --- a/sensors/vehicle_sensor_node.gd +++ b/sensors/vehicle_sensor_node.gd @@ -48,6 +48,7 @@ var heading_rad: float func _ready() -> void: + super._ready() 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) @@ -63,6 +64,12 @@ func _on_gps_data_received(source_ip: String, timestamp_list: Array, new_coordin 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() > 0 and (timestamp - gps_timestamps[-1]) < -1: + gps_timestamps.clear() + coordinates.clear() + gps_positions.clear() + altitudes.clear() + speed_data.clear() 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) @@ -93,6 +100,13 @@ func _on_accel_data_received(source_ip: String, timestamp_list: Array, accel: Ve 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() > 0 and (timestamp - accel_timestamps[-1]) < -1: + accel_timestamps.clear() + linear_vecs.clear() + g_vecs.clear() + pitch_data.clear() + roll_data.clear() + lateral_g_data.clear() if accel_timestamps.size() == max_data_length and not TimeHelpers.is_timestamp_newer_24h(accel_timestamps[-1], timestamp): return var insert_idx = Bisect.reverse_bisect(accel_timestamps, timestamp) @@ -122,6 +136,10 @@ func _on_mag_data_received(source_ip: String, timestamp_list: Array, mag_vec: Ve 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() > 0 and (timestamp - mag_timestamps[-1]) < -1: + mag_timestamps.clear() + mag_vecs.clear() + heading_data.clear() if mag_timestamps.size() == max_data_length and not TimeHelpers.is_timestamp_newer_24h(mag_timestamps[-1], timestamp): return var insert_idx = Bisect.reverse_bisect(mag_timestamps, timestamp) diff --git a/signals/data_signals.gd b/signals/data_signals.gd index 7b391ef..e47d0dc 100644 --- a/signals/data_signals.gd +++ b/signals/data_signals.gd @@ -1,9 +1,11 @@ extends Node +signal raw_data_received(source_ip: String, data_type: String, timestamp_list: Array, data_dict: Dictionary) signal time_data_received(source_ip: String, timestamp_list: Array) signal gps_data_received(source_ip: String, timestamp_list: Array, coordinates: Array, speed_kmh: float, dop: Array) signal sat_data_received(source_ip: String, timestamp: float, sat_count: Array, sat_position: Array) signal accelerometer_data_received(source_ip: String, timestamp: float, accel: Vector3, linear_accel: Vector3, g_accel: Vector3) signal magnetometer_data_received(source_ip: String, timestamp: float, mag_vec: Vector3) signal gyroscope_data_received(source_ip: String, timestamp: float, data: Array) +signal lidar_2d_data_received(sensor_ip: String, timestamp_list: Array, lidar_point_count: int, lidar_data: Array) diff --git a/views/2d/View2D.tscn b/views/2d/View2D.tscn index e4e53d8..07086b8 100644 --- a/views/2d/View2D.tscn +++ b/views/2d/View2D.tscn @@ -1,6 +1,5 @@ [gd_scene format=3 uid="uid://c2yga5koqwo72"] -[ext_resource type="PackedScene" uid="uid://bbdbyjievjpsq" path="res://widgets/compass/compass_widget_scene.tscn" id="1_fyqef"] [ext_resource type="Script" uid="uid://ba7ta7jjrjuhg" path="res://views/2d/view_2d.gd" id="1_n6fjn"] [ext_resource type="PackedScene" uid="uid://b71kuj4yole3j" path="res://widgets/satellites/satellite_widget.tscn" id="2_3b2b5"] [ext_resource type="PackedScene" uid="uid://m14p8xxs14rf" path="res://widgets/altitude/altitude_widget.tscn" id="3_g6l6n"] @@ -14,9 +13,6 @@ 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(104, 486) - [node name="TotalSatCount" parent="HUD" unique_id=931018379 instance=ExtResource("2_3b2b5")] visible = false scale = Vector2(0.05, 0.05) @@ -64,10 +60,10 @@ scale = Vector2(0.05, 0.05) sat_category = "QZSS" [node name="AltitudeWidget" parent="HUD" unique_id=1004159593 instance=ExtResource("3_g6l6n")] -position = Vector2(170, 481) +position = Vector2(233, 479) [node name="AccelerometerWidget" parent="HUD" unique_id=1057256615 instance=ExtResource("4_qui0h")] -position = Vector2(675, 400) +position = Vector2(673, 302) [node name="RollWidget" parent="HUD" unique_id=1446458625 instance=ExtResource("6_s8d37")] position = Vector2(407, 267) diff --git a/views/2d/test_lidar.gd b/views/2d/test_lidar.gd new file mode 100644 index 0000000..4e7d117 --- /dev/null +++ b/views/2d/test_lidar.gd @@ -0,0 +1,39 @@ +extends Node2D + +var line: Line2D + +var points_angle = [] +var points_distance = [] +var timer_static = 60 +var timer = 0. + +# Called when the node enters the scene tree for the first time. +func _ready() -> void: + line = get_node('Line2D') + DataSignals.lidar_2d_data_received.connect(_on_lidar_data_received) + + +# Called every frame. 'delta' is the elapsed time since the previous frame. +func _process(delta: float) -> void: + timer += delta + if timer > timer_static: + timer = 0 + points_angle.clear() + points_distance.clear() + for i in line.points: + print(i - Vector2(400, 400)) + print() + _update_line() + +func _on_lidar_data_received(sensor_ip: String, timestamp_list: Array, lidar_point_count: int, lidar_data: Array): + for point in lidar_data: + var insert_idx = Bisect.bisect(points_angle, point[0]) + points_angle.insert(insert_idx, point[0]) + points_distance.insert(insert_idx, point[1]) + +func _update_line(): + line.clear_points() + for idx in range(points_angle.size()): + var x = 4*points_distance[idx] * cos(deg_to_rad(points_angle[idx])) + 400 + var y = 4*points_distance[idx] * sin(deg_to_rad(points_angle[idx])) + 400 + line.add_point(Vector2(x, y)) diff --git a/views/2d/test_lidar.gd.uid b/views/2d/test_lidar.gd.uid new file mode 100644 index 0000000..c2a85de --- /dev/null +++ b/views/2d/test_lidar.gd.uid @@ -0,0 +1 @@ +uid://cei6fgt3rsc2c diff --git a/views/2d/test_lidar.tscn b/views/2d/test_lidar.tscn new file mode 100644 index 0000000..fe0aa13 --- /dev/null +++ b/views/2d/test_lidar.tscn @@ -0,0 +1,13 @@ +[gd_scene format=3 uid="uid://ch0lyqkkxaem"] + +[ext_resource type="Script" uid="uid://cei6fgt3rsc2c" path="res://views/2d/test_lidar.gd" id="1_8h8gx"] + +[node name="TestLidar" type="Node2D" unique_id=948964638] +script = ExtResource("1_8h8gx") + +[node name="Camera2D" type="Camera2D" parent="." unique_id=60160156] +position = Vector2(400, 400) + +[node name="Line2D" type="Line2D" parent="." unique_id=1424623383] +closed = true +width = 1.0 diff --git a/widgets/accelerometer/accelerometer_widget.tscn b/widgets/accelerometer/accelerometer_widget.tscn index 7b7426f..29f068d 100644 --- a/widgets/accelerometer/accelerometer_widget.tscn +++ b/widgets/accelerometer/accelerometer_widget.tscn @@ -7,6 +7,16 @@ script = ExtResource("1_hqf45") metadata/_custom_type_script = "uid://cr4oydwvmm28y" +[node name="Label" type="Label" parent="." unique_id=749031600] +custom_minimum_size = Vector2(200, 0) +offset_left = -100.0 +offset_top = 90.0 +offset_right = 100.0 +offset_bottom = 178.0 +theme_override_font_sizes/font_size = 64 +text = "10.0 g" +horizontal_alignment = 1 + [node name="Panel" type="Panel" parent="." unique_id=1154284634] visible = false offset_left = -103.0 @@ -32,17 +42,13 @@ default_color = Color(0, 1, 0, 1) begin_cap_mode = 2 end_cap_mode = 2 -[node name="Label" type="Label" parent="." unique_id=749031600] -offset_right = 40.0 -offset_bottom = 23.0 - [node name="MovableControl" type="Control" parent="." unique_id=345939378 node_paths=PackedStringArray("move_target")] layout_mode = 3 anchors_preset = 0 offset_left = -103.0 offset_top = -103.0 offset_right = 106.0 -offset_bottom = 106.0 +offset_bottom = 183.0 mouse_filter = 1 script = ExtResource("2_pog3i") move_target = NodePath("..") diff --git a/widgets/accelerometer/scripts/accelerometer_widget_node.gd b/widgets/accelerometer/scripts/accelerometer_widget_node.gd index ee31a52..db08195 100644 --- a/widgets/accelerometer/scripts/accelerometer_widget_node.gd +++ b/widgets/accelerometer/scripts/accelerometer_widget_node.gd @@ -24,8 +24,9 @@ func _update_data(): 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] + var end_point = Vector2(lateral_gs.y, -lateral_gs.x) + var forward = Vector2(0, -lateral_gs.x) + accel_vector.points = [origin, end_point * 4] + x_vector.points = [forward * 4, end_point * 4] + y_vector.points = [origin, forward * 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 d6b5261..5479681 100644 --- a/widgets/altitude/altitude_widget.tscn +++ b/widgets/altitude/altitude_widget.tscn @@ -8,21 +8,21 @@ script = ExtResource("1_hmdxp") metadata/_custom_type_script = "uid://b3rfje1auw6to" [node name="Panel" type="Panel" parent="." unique_id=1393400967] -offset_right = 460.0 +offset_right = 300.0 offset_bottom = 120.0 [node name="Label" type="Label" parent="." unique_id=1070194714] -offset_right = 460.0 +offset_right = 300.0 offset_bottom = 120.0 -theme_override_font_sizes/font_size = 80 -text = "Alt: 99999 ft" +theme_override_font_sizes/font_size = 64 +text = "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 = 460.0 +offset_right = 300.0 offset_bottom = 120.0 mouse_filter = 1 script = ExtResource("2_34sps") diff --git a/widgets/altitude/scripts/altitude_widget_node.gd b/widgets/altitude/scripts/altitude_widget_node.gd index 82b84d2..b1f222b 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 = 'Alt: ' + str(int(round(host_vehicle.altitude_m * Constants.M2FT))) + ' ft' + label_node.text = str(int(round(host_vehicle.altitude_m * Constants.M2FT))) + ' ft' diff --git a/widgets/lidar_2d/lidar_2d.tscn b/widgets/lidar_2d/lidar_2d.tscn new file mode 100644 index 0000000..9e31a4a --- /dev/null +++ b/widgets/lidar_2d/lidar_2d.tscn @@ -0,0 +1,3 @@ +[gd_scene format=3 uid="uid://dhxuhkb8inhqa"] + +[node name="Lidar2d" type="Node3D" unique_id=679038150] diff --git a/widgets/rotation/pitch_widget.tscn b/widgets/rotation/pitch_widget.tscn index 2d03bf0..a406171 100644 --- a/widgets/rotation/pitch_widget.tscn +++ b/widgets/rotation/pitch_widget.tscn @@ -26,7 +26,7 @@ offset_left = -50.0 offset_top = -22.0 offset_right = 50.0 offset_bottom = 23.0 -theme_override_font_sizes/font_size = 32 +theme_override_font_sizes/font_size = 48 horizontal_alignment = 1 [node name="Line2D" type="Line2D" parent="." unique_id=1285997474] diff --git a/widgets/rotation/roll_widget.tscn b/widgets/rotation/roll_widget.tscn index ae97ce8..a52683f 100644 --- a/widgets/rotation/roll_widget.tscn +++ b/widgets/rotation/roll_widget.tscn @@ -26,7 +26,7 @@ offset_left = -50.0 offset_top = -22.0 offset_right = 50.0 offset_bottom = 23.0 -theme_override_font_sizes/font_size = 32 +theme_override_font_sizes/font_size = 48 horizontal_alignment = 1 [node name="Line2D" type="Line2D" parent="." unique_id=1446923009] diff --git a/widgets/rotation/scripts/pitch_widget_node.gd b/widgets/rotation/scripts/pitch_widget_node.gd index c9f4a85..b320bd5 100644 --- a/widgets/rotation/scripts/pitch_widget_node.gd +++ b/widgets/rotation/scripts/pitch_widget_node.gd @@ -16,4 +16,4 @@ func _update_data(): if sprite == null or label == null: return sprite.rotation = host_vehicle.pitch_rad - label.text = str(round(rad_to_deg(host_vehicle.pitch_rad)*10)/10) + " \u00B0" + label.text = str(int(rad_to_deg(host_vehicle.pitch_rad))) + " \u00B0" diff --git a/widgets/rotation/scripts/roll_widget_node.gd b/widgets/rotation/scripts/roll_widget_node.gd index ea13430..db12227 100644 --- a/widgets/rotation/scripts/roll_widget_node.gd +++ b/widgets/rotation/scripts/roll_widget_node.gd @@ -15,4 +15,4 @@ func _update_data(): if sprite == null or label == null: return sprite.rotation = host_vehicle.roll_rad - label.text = str(abs(round(rad_to_deg(host_vehicle.roll_rad)*10)/10)) + " \u00B0" + label.text = str(abs(int(rad_to_deg(host_vehicle.roll_rad)))) + " \u00B0" diff --git a/widgets/speed/speed_widget.tscn b/widgets/speed/speed_widget.tscn index 5b2914b..9842ab1 100644 --- a/widgets/speed/speed_widget.tscn +++ b/widgets/speed/speed_widget.tscn @@ -14,7 +14,7 @@ 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 = 80 +theme_override_font_sizes/font_size = 64 text = "10" horizontal_alignment = 1