From f3d38c756901bb7d13719d8ae0a5bfffd57606a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marek=20S=2E=20=C5=81ukasiewicz?= Date: Sat, 13 Dec 2025 21:44:41 +0100 Subject: [PATCH 1/2] Minor MAVLink fixes --- src/marshconnector.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/marshconnector.cpp b/src/marshconnector.cpp index 3f66679..4d348c4 100644 --- a/src/marshconnector.cpp +++ b/src/marshconnector.cpp @@ -26,6 +26,7 @@ #include "mavlink/mavlink_types.h" #include #include +#include using namespace godot; @@ -403,10 +404,11 @@ Error MarshConnector::send_param(Parameter index) { } mavlink_param_value_t param_value; - memset(param_value.param_id, 0, sizeof(param_value.param_id)); CharString nameBuffer = parameter_names[index].ascii(); const char *name = nameBuffer.get_data(); - strcpy_s(param_value.param_id, name); + + // strncpy will write nulls after data until reaching target count + strncpy(param_value.param_id, name, sizeof(param_value.param_id)); param_value.param_value = parameters[index]; param_value.param_type = MAV_PARAM_TYPE_REAL32; param_value.param_count = PARAM_COUNT; @@ -447,9 +449,8 @@ void MarshConnector::handle_heartbeat(mavlink_message_t message) { // subscribe to messages not sent to visualisation node by default mavlink_command_long_t command; - command.target_system = 1; - command.target_component = - MAV_COMP_ID_USER1; // HACK: Should get it from received HEARTBEAT + command.target_system = message.sysid; + command.target_component = message.compid; command.command = MAV_CMD_SET_MESSAGE_INTERVAL; command.confirmation = 0; command.param1 = static_cast(MAVLINK_MSG_ID_MANUAL_CONTROL); @@ -458,7 +459,7 @@ void MarshConnector::handle_heartbeat(mavlink_message_t message) { command.param4 = 0; command.param5 = 0; command.param6 = 0; - command.param7 = 1; // Address of requestor + command.param7 = 1; // Address of requester mavlink_message_t message_sent; mavlink_msg_command_long_encode_chan( From 821072eae571f1e6f3d62074f15922e4ab93e1e1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marek=20S=2E=20=C5=81ukasiewicz?= Date: Sun, 14 Dec 2025 00:12:56 +0100 Subject: [PATCH 2/2] Recreate lidia controls with Node2D --- project/aircraft/aircraft.gd | 6 ++ project/aircraft/aircraft.tscn | 2 +- project/instruments.gd | 15 ++-- project/instruments/controls-background.png | 3 + .../controls-background.png.import | 34 ++++++++ project/instruments/controls-ref.png | 3 + project/instruments/controls-ref.png.import | 34 ++++++++ project/instruments/controls.gd | 33 ++++++++ project/instruments/controls.gd.uid | 1 + project/instruments/controls.tscn | 54 +++++++++++++ project/instruments/current-arrow.png | 3 + project/instruments/current-arrow.png.import | 34 ++++++++ project/instruments/current-dot.png | 3 + project/instruments/current-dot.png.import | 34 ++++++++ project/{ => instruments}/instruments.tscn | 12 ++- project/instruments/pfd-ref.png | 3 + project/instruments/pfd-ref.png.import | 34 ++++++++ project/instruments/trim-long.png | 3 + project/instruments/trim-long.png.import | 34 ++++++++ project/instruments/trim-short.png | 3 + project/instruments/trim-short.png.import | 34 ++++++++ src/marshconnector.cpp | 77 ++++++++++++++----- src/marshconnector.h | 9 +++ 23 files changed, 433 insertions(+), 35 deletions(-) create mode 100644 project/instruments/controls-background.png create mode 100644 project/instruments/controls-background.png.import create mode 100644 project/instruments/controls-ref.png create mode 100644 project/instruments/controls-ref.png.import create mode 100644 project/instruments/controls.gd create mode 100644 project/instruments/controls.gd.uid create mode 100644 project/instruments/controls.tscn create mode 100644 project/instruments/current-arrow.png create mode 100644 project/instruments/current-arrow.png.import create mode 100644 project/instruments/current-dot.png create mode 100644 project/instruments/current-dot.png.import rename project/{ => instruments}/instruments.tscn (82%) create mode 100644 project/instruments/pfd-ref.png create mode 100644 project/instruments/pfd-ref.png.import create mode 100644 project/instruments/trim-long.png create mode 100644 project/instruments/trim-long.png.import create mode 100644 project/instruments/trim-short.png create mode 100644 project/instruments/trim-short.png.import diff --git a/project/aircraft/aircraft.gd b/project/aircraft/aircraft.gd index 9ecfbde..3870b93 100644 --- a/project/aircraft/aircraft.gd +++ b/project/aircraft/aircraft.gd @@ -25,6 +25,8 @@ var _rotor_azimuth: float = 0 @onready var bone_cyclic: int = skeleton.find_bone("Cyclic") @onready var bone_rotor: int = skeleton.find_bone("Rotor") +@onready var instruments: Node = $AttitudeRoot/Instruments + func _process(delta: float) -> void: var target: Transform3D = connector.get_aircraft() position = target.origin @@ -43,6 +45,10 @@ func _process(delta: float) -> void: var cyc_angles_rad: Vector2 = cyclic * deg_to_rad(range_cyclic) var cyc_att := Quaternion.from_euler(Vector3(cyc_angles_rad.y, cyc_angles_rad.x, 0)) skeleton.set_bone_pose_rotation(bone_cyclic, cyc_att * cyc_rest) + + # Update the instruments + instruments.call("set_controls", connector.get_controls()) + instruments.call("set_trim", connector.get_trim()) # Spin the rotor only when receiving flight data if connector.get_model_connected(): diff --git a/project/aircraft/aircraft.tscn b/project/aircraft/aircraft.tscn index d572319..484b3e1 100644 --- a/project/aircraft/aircraft.tscn +++ b/project/aircraft/aircraft.tscn @@ -3,7 +3,7 @@ [ext_resource type="Script" uid="uid://cx30pr7kn4c74" path="res://aircraft/aircraft.gd" id="1_l4uib"] [ext_resource type="PackedScene" uid="uid://cux4tju0ovvly" path="res://assets/mi2/Mi-2.glb" id="1_mrxe8"] [ext_resource type="PackedScene" uid="uid://fmygcraoturj" path="res://reference_axes.tscn" id="3_2bi7g"] -[ext_resource type="PackedScene" uid="uid://cis4s43ubuynp" path="res://instruments.tscn" id="3_5w717"] +[ext_resource type="PackedScene" uid="uid://cis4s43ubuynp" path="res://instruments/instruments.tscn" id="3_5w717"] [ext_resource type="Script" uid="uid://bt32fse84itrp" path="res://aircraft/fallback_input.gd" id="3_gf6ud"] [sub_resource type="StandardMaterial3D" id="StandardMaterial3D_mrxe8"] diff --git a/project/instruments.gd b/project/instruments.gd index 28a25e6..a50183e 100644 --- a/project/instruments.gd +++ b/project/instruments.gd @@ -1,11 +1,12 @@ extends Node const PFD_PATH: String = "pfd#adi,vsi,alt,ias,rht,rpm,hsi" -const CONTROLS_PATH: String = "controls#collective,cyclic,rudder" @export var lidia_hostname: String = "localhost" @export var lidia_port: int = 5555 +@onready var controls: Node = $SubViewport2/Controls + func _ready(): if !$CEF.initialize({"incognito":true, "locale":"en-US"}): push_error($CEF.get_error()) @@ -24,10 +25,8 @@ func _ready(): browser_pfd.name = "pfd" browser_pfd.enable_ad_block(false) # Required for lidia static assets - var browser_ctrl = $CEF.create_browser( - "http://{}:{}/{}".format([lidia_hostname, lidia_port, CONTROLS_PATH], "{}"), - $SubViewport2/TextureRect, - { "frame_rate": 90, "javascript": true }, - ) - browser_ctrl.name = "controls" - browser_ctrl.enable_ad_block(false) # Required for lidia static assets +# Propagate the calls to child +func set_controls(current: Vector4): + controls.call("set_controls", current) +func set_trim(trim: Vector4): + controls.call("set_trim", trim) diff --git a/project/instruments/controls-background.png b/project/instruments/controls-background.png new file mode 100644 index 0000000..01648dd --- /dev/null +++ b/project/instruments/controls-background.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:94045c4fc08b5d93d3a2436de2c8399341246ec157f8ac3dbf0a89806a9e0e91 +size 2842 diff --git a/project/instruments/controls-background.png.import b/project/instruments/controls-background.png.import new file mode 100644 index 0000000..b6fb2d4 --- /dev/null +++ b/project/instruments/controls-background.png.import @@ -0,0 +1,34 @@ +[remap] + +importer="texture" +type="CompressedTexture2D" +uid="uid://b0oeokas7mq4q" +path="res://.godot/imported/controls-background.png-626db610a70b7b467da1123d55529d5c.ctex" +metadata={ +"vram_texture": false +} + +[deps] + +source_file="res://instruments/controls-background.png" +dest_files=["res://.godot/imported/controls-background.png-626db610a70b7b467da1123d55529d5c.ctex"] + +[params] + +compress/mode=0 +compress/high_quality=false +compress/lossy_quality=0.7 +compress/hdr_compression=1 +compress/normal_map=0 +compress/channel_pack=0 +mipmaps/generate=false +mipmaps/limit=-1 +roughness/mode=0 +roughness/src_normal="" +process/fix_alpha_border=true +process/premult_alpha=false +process/normal_map_invert_y=false +process/hdr_as_srgb=false +process/hdr_clamp_exposure=false +process/size_limit=0 +detect_3d/compress_to=1 diff --git a/project/instruments/controls-ref.png b/project/instruments/controls-ref.png new file mode 100644 index 0000000..f4f3726 --- /dev/null +++ b/project/instruments/controls-ref.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4eef65cc3a825494e9d405ea68c480de3da6fe2e62f202e5d387771331d2904f +size 15488 diff --git a/project/instruments/controls-ref.png.import b/project/instruments/controls-ref.png.import new file mode 100644 index 0000000..f623b6a --- /dev/null +++ b/project/instruments/controls-ref.png.import @@ -0,0 +1,34 @@ +[remap] + +importer="texture" +type="CompressedTexture2D" +uid="uid://c5bcvbphgg1nk" +path="res://.godot/imported/controls-ref.png-ed1b9d803223424fa895ad19151b7e9f.ctex" +metadata={ +"vram_texture": false +} + +[deps] + +source_file="res://instruments/controls-ref.png" +dest_files=["res://.godot/imported/controls-ref.png-ed1b9d803223424fa895ad19151b7e9f.ctex"] + +[params] + +compress/mode=0 +compress/high_quality=false +compress/lossy_quality=0.7 +compress/hdr_compression=1 +compress/normal_map=0 +compress/channel_pack=0 +mipmaps/generate=false +mipmaps/limit=-1 +roughness/mode=0 +roughness/src_normal="" +process/fix_alpha_border=true +process/premult_alpha=false +process/normal_map_invert_y=false +process/hdr_as_srgb=false +process/hdr_clamp_exposure=false +process/size_limit=0 +detect_3d/compress_to=1 diff --git a/project/instruments/controls.gd b/project/instruments/controls.gd new file mode 100644 index 0000000..86b7a7b --- /dev/null +++ b/project/instruments/controls.gd @@ -0,0 +1,33 @@ +extends Node2D +## Recreate the controls page from lidia Python package + +@onready var trim_longitudinal: Node2D = $CyclicOrigin/TrimLongitudinal +@onready var trim_lateral: Node2D = $CyclicOrigin/TrimLateral +@onready var current_cyclic: Node2D = $CyclicOrigin/CurrentCyclic +@onready var trim_collective: Node2D = $CollectiveOrigin/TrimCollective +@onready var current_collective: Node2D = $CollectiveOrigin/CurrentCollective +@onready var trim_pedals: Node2D = $PedalsOrigin/TrimPedals +@onready var current_pedals: Node2D = $PedalsOrigin/CurrentPedals + +const AXIS_SIZE: float = 326 + +func set_controls(current: Vector4): + set_cyclic(Vector2(current.x, current.y)) + set_collective(current.w) + set_pedals(current.z) + +func set_cyclic(current: Vector2): + current_cyclic.position.x = AXIS_SIZE / 2.0 * current.x + current_cyclic.position.y = AXIS_SIZE / 2.0 * current.y + +func set_collective(current: float): + current_collective.position.y = -AXIS_SIZE * current + +func set_pedals(current: float): + current_pedals.position.x = AXIS_SIZE / 2.0 * current + +func set_trim(trim: Vector4): + trim_lateral.position.x = AXIS_SIZE / 2.0 * trim.x + trim_longitudinal.position.y = AXIS_SIZE / 2.0 * trim.y + trim_pedals.position.x = AXIS_SIZE / 2.0 * trim.z + trim_collective.position.y = -AXIS_SIZE * trim.w diff --git a/project/instruments/controls.gd.uid b/project/instruments/controls.gd.uid new file mode 100644 index 0000000..0a62356 --- /dev/null +++ b/project/instruments/controls.gd.uid @@ -0,0 +1 @@ +uid://ds6f8cur05ka0 diff --git a/project/instruments/controls.tscn b/project/instruments/controls.tscn new file mode 100644 index 0000000..be77aeb --- /dev/null +++ b/project/instruments/controls.tscn @@ -0,0 +1,54 @@ +[gd_scene load_steps=7 format=3 uid="uid://bgkpwebqksth5"] + +[ext_resource type="Script" uid="uid://ds6f8cur05ka0" path="res://instruments/controls.gd" id="1_g7kvs"] +[ext_resource type="Texture2D" uid="uid://b0oeokas7mq4q" path="res://instruments/controls-background.png" id="2_wbvwt"] +[ext_resource type="Texture2D" uid="uid://c561m3pvc0kop" path="res://instruments/trim-long.png" id="3_37ekc"] +[ext_resource type="Texture2D" uid="uid://wdqnlguxmi2a" path="res://instruments/current-dot.png" id="4_4updv"] +[ext_resource type="Texture2D" uid="uid://dytpn8rcdimr" path="res://instruments/trim-short.png" id="5_ni3tk"] +[ext_resource type="Texture2D" uid="uid://dkda6ju5daocf" path="res://instruments/current-arrow.png" id="6_mfntf"] + +[node name="Controls" type="Node2D"] +script = ExtResource("1_g7kvs") + +[node name="Background" type="Sprite2D" parent="."] +position = Vector2(256, 256) +texture = ExtResource("2_wbvwt") + +[node name="CyclicOrigin" type="Node2D" parent="."] +position = Vector2(309, 189) + +[node name="TrimLongitudinal" type="Sprite2D" parent="CyclicOrigin"] +position = Vector2(10, 0) +texture = ExtResource("3_37ekc") + +[node name="TrimLateral" type="Sprite2D" parent="CyclicOrigin"] +position = Vector2(0, 10) +rotation = 1.5708 +texture = ExtResource("3_37ekc") + +[node name="CurrentCyclic" type="Sprite2D" parent="CyclicOrigin"] +texture = ExtResource("4_4updv") + +[node name="CollectiveOrigin" type="Node2D" parent="."] +position = Vector2(72.5, 352) + +[node name="TrimCollective" type="Sprite2D" parent="CollectiveOrigin"] +position = Vector2(-10.5, 0) +texture = ExtResource("5_ni3tk") + +[node name="CurrentCollective" type="Sprite2D" parent="CollectiveOrigin"] +position = Vector2(20.5, 0) +texture = ExtResource("6_mfntf") + +[node name="PedalsOrigin" type="Node2D" parent="."] +position = Vector2(309, 425.5) + +[node name="TrimPedals" type="Sprite2D" parent="PedalsOrigin"] +position = Vector2(0, 10.5) +rotation = -1.5708 +texture = ExtResource("5_ni3tk") + +[node name="CurrentPedals" type="Sprite2D" parent="PedalsOrigin"] +position = Vector2(0, -21.5) +rotation = -1.5708 +texture = ExtResource("6_mfntf") diff --git a/project/instruments/current-arrow.png b/project/instruments/current-arrow.png new file mode 100644 index 0000000..e4aa8e4 --- /dev/null +++ b/project/instruments/current-arrow.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e9f32544518014aa8fbb852c82b5a9a32e78cc96724a095e261f3bb870602e4a +size 1228 diff --git a/project/instruments/current-arrow.png.import b/project/instruments/current-arrow.png.import new file mode 100644 index 0000000..430c1a0 --- /dev/null +++ b/project/instruments/current-arrow.png.import @@ -0,0 +1,34 @@ +[remap] + +importer="texture" +type="CompressedTexture2D" +uid="uid://dkda6ju5daocf" +path="res://.godot/imported/current-arrow.png-8e092288abc9ec8d31cbc8f708a9c800.ctex" +metadata={ +"vram_texture": false +} + +[deps] + +source_file="res://instruments/current-arrow.png" +dest_files=["res://.godot/imported/current-arrow.png-8e092288abc9ec8d31cbc8f708a9c800.ctex"] + +[params] + +compress/mode=0 +compress/high_quality=false +compress/lossy_quality=0.7 +compress/hdr_compression=1 +compress/normal_map=0 +compress/channel_pack=0 +mipmaps/generate=false +mipmaps/limit=-1 +roughness/mode=0 +roughness/src_normal="" +process/fix_alpha_border=true +process/premult_alpha=false +process/normal_map_invert_y=false +process/hdr_as_srgb=false +process/hdr_clamp_exposure=false +process/size_limit=0 +detect_3d/compress_to=1 diff --git a/project/instruments/current-dot.png b/project/instruments/current-dot.png new file mode 100644 index 0000000..c7eea25 --- /dev/null +++ b/project/instruments/current-dot.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ac4ea7768fdae5a5f8e6f2ab7d48d332a6ab09f78d520c4fe0404c414aaf1b69 +size 1047 diff --git a/project/instruments/current-dot.png.import b/project/instruments/current-dot.png.import new file mode 100644 index 0000000..ce845b9 --- /dev/null +++ b/project/instruments/current-dot.png.import @@ -0,0 +1,34 @@ +[remap] + +importer="texture" +type="CompressedTexture2D" +uid="uid://wdqnlguxmi2a" +path="res://.godot/imported/current-dot.png-789da8759af9526ce5f8705121a1077d.ctex" +metadata={ +"vram_texture": false +} + +[deps] + +source_file="res://instruments/current-dot.png" +dest_files=["res://.godot/imported/current-dot.png-789da8759af9526ce5f8705121a1077d.ctex"] + +[params] + +compress/mode=0 +compress/high_quality=false +compress/lossy_quality=0.7 +compress/hdr_compression=1 +compress/normal_map=0 +compress/channel_pack=0 +mipmaps/generate=false +mipmaps/limit=-1 +roughness/mode=0 +roughness/src_normal="" +process/fix_alpha_border=true +process/premult_alpha=false +process/normal_map_invert_y=false +process/hdr_as_srgb=false +process/hdr_clamp_exposure=false +process/size_limit=0 +detect_3d/compress_to=1 diff --git a/project/instruments.tscn b/project/instruments/instruments.tscn similarity index 82% rename from project/instruments.tscn rename to project/instruments/instruments.tscn index 4ececc9..1a4f7fe 100644 --- a/project/instruments.tscn +++ b/project/instruments/instruments.tscn @@ -1,6 +1,7 @@ -[gd_scene load_steps=7 format=3 uid="uid://cis4s43ubuynp"] +[gd_scene load_steps=8 format=3 uid="uid://cis4s43ubuynp"] -[ext_resource type="Script" uid="uid://01bmfj4wthwg" path="res://instruments.gd" id="1_h5at3"] +[ext_resource type="Script" uid="uid://01bmfj4wthwg" path="res://instruments.gd" id="1_wlkep"] +[ext_resource type="PackedScene" uid="uid://bgkpwebqksth5" path="res://instruments/controls.tscn" id="2_372d7"] [sub_resource type="QuadMesh" id="QuadMesh_nowl7"] @@ -19,7 +20,7 @@ resource_local_to_scene = true albedo_texture = SubResource("ViewportTexture_h5at3") [node name="Instruments" type="Node3D"] -script = ExtResource("1_h5at3") +script = ExtResource("1_wlkep") lidia_hostname = "192.168.1.2" [node name="SubViewport" type="SubViewport" parent="."] @@ -36,10 +37,7 @@ surface_material_override/0 = SubResource("StandardMaterial3D_h5at3") [node name="SubViewport2" type="SubViewport" parent="."] -[node name="TextureRect" type="TextureRect" parent="SubViewport2"] -offset_right = 512.0 -offset_bottom = 512.0 -expand_mode = 5 +[node name="Controls" parent="SubViewport2" instance=ExtResource("2_372d7")] [node name="Quad2" type="MeshInstance3D" parent="."] transform = Transform3D(0.6, 0, 0, 0, 0.6, 0, 0, 0, 0.6, 0.425, 0, 0) diff --git a/project/instruments/pfd-ref.png b/project/instruments/pfd-ref.png new file mode 100644 index 0000000..08a7309 --- /dev/null +++ b/project/instruments/pfd-ref.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3e079dc36171686e572fe77691d340b82e43288c4b6aaf81776158162e3e50ce +size 70065 diff --git a/project/instruments/pfd-ref.png.import b/project/instruments/pfd-ref.png.import new file mode 100644 index 0000000..3f31bfa --- /dev/null +++ b/project/instruments/pfd-ref.png.import @@ -0,0 +1,34 @@ +[remap] + +importer="texture" +type="CompressedTexture2D" +uid="uid://toojp8r3424r" +path="res://.godot/imported/pfd-ref.png-18e0fc5274406321f68279cb4defcf25.ctex" +metadata={ +"vram_texture": false +} + +[deps] + +source_file="res://instruments/pfd-ref.png" +dest_files=["res://.godot/imported/pfd-ref.png-18e0fc5274406321f68279cb4defcf25.ctex"] + +[params] + +compress/mode=0 +compress/high_quality=false +compress/lossy_quality=0.7 +compress/hdr_compression=1 +compress/normal_map=0 +compress/channel_pack=0 +mipmaps/generate=false +mipmaps/limit=-1 +roughness/mode=0 +roughness/src_normal="" +process/fix_alpha_border=true +process/premult_alpha=false +process/normal_map_invert_y=false +process/hdr_as_srgb=false +process/hdr_clamp_exposure=false +process/size_limit=0 +detect_3d/compress_to=1 diff --git a/project/instruments/trim-long.png b/project/instruments/trim-long.png new file mode 100644 index 0000000..0a52de6 --- /dev/null +++ b/project/instruments/trim-long.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6f57cf6e93c3c68e48afdc3ee078c28c9f3292d4303a7fcc0ec51e1651d33e3a +size 2108 diff --git a/project/instruments/trim-long.png.import b/project/instruments/trim-long.png.import new file mode 100644 index 0000000..fbfafb1 --- /dev/null +++ b/project/instruments/trim-long.png.import @@ -0,0 +1,34 @@ +[remap] + +importer="texture" +type="CompressedTexture2D" +uid="uid://c561m3pvc0kop" +path="res://.godot/imported/trim-long.png-0b9b67642209f6a2ae4b2165f3b33d5d.ctex" +metadata={ +"vram_texture": false +} + +[deps] + +source_file="res://instruments/trim-long.png" +dest_files=["res://.godot/imported/trim-long.png-0b9b67642209f6a2ae4b2165f3b33d5d.ctex"] + +[params] + +compress/mode=0 +compress/high_quality=false +compress/lossy_quality=0.7 +compress/hdr_compression=1 +compress/normal_map=0 +compress/channel_pack=0 +mipmaps/generate=false +mipmaps/limit=-1 +roughness/mode=0 +roughness/src_normal="" +process/fix_alpha_border=true +process/premult_alpha=false +process/normal_map_invert_y=false +process/hdr_as_srgb=false +process/hdr_clamp_exposure=false +process/size_limit=0 +detect_3d/compress_to=1 diff --git a/project/instruments/trim-short.png b/project/instruments/trim-short.png new file mode 100644 index 0000000..f54ccfb --- /dev/null +++ b/project/instruments/trim-short.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fa70ce3b0fa0c5716ba0e771877eb8499cdd33d8b0c7478bea1c91b79b32e6b1 +size 1265 diff --git a/project/instruments/trim-short.png.import b/project/instruments/trim-short.png.import new file mode 100644 index 0000000..9777f63 --- /dev/null +++ b/project/instruments/trim-short.png.import @@ -0,0 +1,34 @@ +[remap] + +importer="texture" +type="CompressedTexture2D" +uid="uid://dytpn8rcdimr" +path="res://.godot/imported/trim-short.png-707c75166ad2d77e18c02dc4882968b1.ctex" +metadata={ +"vram_texture": false +} + +[deps] + +source_file="res://instruments/trim-short.png" +dest_files=["res://.godot/imported/trim-short.png-707c75166ad2d77e18c02dc4882968b1.ctex"] + +[params] + +compress/mode=0 +compress/high_quality=false +compress/lossy_quality=0.7 +compress/hdr_compression=1 +compress/normal_map=0 +compress/channel_pack=0 +mipmaps/generate=false +mipmaps/limit=-1 +roughness/mode=0 +roughness/src_normal="" +process/fix_alpha_border=true +process/premult_alpha=false +process/normal_map_invert_y=false +process/hdr_as_srgb=false +process/hdr_clamp_exposure=false +process/size_limit=0 +detect_3d/compress_to=1 diff --git a/src/marshconnector.cpp b/src/marshconnector.cpp index 4d348c4..fa2a9b9 100644 --- a/src/marshconnector.cpp +++ b/src/marshconnector.cpp @@ -18,6 +18,7 @@ #include "mavlink/all/mavlink.h" // IWYU pragma: keep; always include the mavlink.h file for selected dialect #include "mavlink/common/mavlink_msg_command_long.h" #include "mavlink/common/mavlink_msg_manual_control.h" +#include "mavlink/common/mavlink_msg_manual_setpoint.h" #include "mavlink/common/mavlink_msg_param_request_list.h" #include "mavlink/common/mavlink_msg_param_set.h" #include "mavlink/common/mavlink_msg_param_value.h" @@ -37,6 +38,8 @@ void MarshConnector::_bind_methods() { ClassDB::bind_method(D_METHOD("get_collective"), &MarshConnector::get_collective); ClassDB::bind_method(D_METHOD("get_pedals"), &MarshConnector::get_pedals); + ClassDB::bind_method(D_METHOD("get_controls"), &MarshConnector::get_controls); + ClassDB::bind_method(D_METHOD("get_trim"), &MarshConnector::get_trim); ClassDB::bind_method(D_METHOD("get_model_connected"), &MarshConnector::get_model_connected); ClassDB::bind_method(D_METHOD("get_manager_connected"), @@ -182,6 +185,9 @@ void MarshConnector::receive_data(const PackedByteArray &data) { case MAVLINK_MSG_ID_MANUAL_CONTROL: handle_manual_control(message); break; + case MAVLINK_MSG_ID_MANUAL_SETPOINT: + handle_manual_setpoint(message); + break; case MAVLINK_MSG_ID_HEARTBEAT: handle_heartbeat(message); break; @@ -244,6 +250,10 @@ float MarshConnector::get_collective() { return last_controls.w; } float MarshConnector::get_pedals() { return last_controls.z; } +Vector4 MarshConnector::get_controls() { return last_controls; } + +Vector4 MarshConnector::get_trim() { return last_trim; } + bool MarshConnector::get_manager_connected() { return manager_connected; } bool MarshConnector::get_model_connected() { return model_connected; } @@ -437,6 +447,22 @@ void MarshConnector::handle_manual_control(mavlink_message_t message) { last_controls.w = manual_control.z / 1000.0f; } +void MarshConnector::handle_manual_setpoint(mavlink_message_t message) { + if (message.msgid != MAVLINK_MSG_ID_MANUAL_SETPOINT) + return; + + mavlink_manual_setpoint_t manual_setpoint; + mavlink_msg_manual_setpoint_decode(&message, &manual_setpoint); + if (manual_setpoint.mode_switch == MARSH_MANUAL_SETPOINT_MODE_TRIM) { + last_trim = Vector4{ + manual_setpoint.roll, + manual_setpoint.pitch, + manual_setpoint.yaw, + manual_setpoint.thrust, + }; + } +} + void MarshConnector::handle_heartbeat(mavlink_message_t message) { if (message.msgid != MAVLINK_MSG_ID_HEARTBEAT) return; @@ -447,26 +473,16 @@ void MarshConnector::handle_heartbeat(mavlink_message_t message) { if (!manager_connected) { print_line("Connected to MARSH Manager"); - // subscribe to messages not sent to visualisation node by default - mavlink_command_long_t command; - command.target_system = message.sysid; - command.target_component = message.compid; - command.command = MAV_CMD_SET_MESSAGE_INTERVAL; - command.confirmation = 0; - command.param1 = static_cast(MAVLINK_MSG_ID_MANUAL_CONTROL); - command.param2 = 0; // Default rate - command.param3 = 0; // Not used - command.param4 = 0; - command.param5 = 0; - command.param6 = 0; - command.param7 = 1; // Address of requester - - mavlink_message_t message_sent; - mavlink_msg_command_long_encode_chan( - system_id, component_id, MAVLINK_COMM_0, &message_sent, &command); - Error result = send_message(message_sent); + Error result = subscribe_message(message.sysid, message.compid, + MAVLINK_MSG_ID_MANUAL_CONTROL); if (result != OK) { - print_line("Subscribe send result ", result); + print_line("Subscribe control send result ", result); + } + + result = subscribe_message(message.sysid, message.compid, + MAVLINK_MSG_ID_MANUAL_SETPOINT); + if (result != OK) { + print_line("Subscribe setpoint send result ", result); } } @@ -475,6 +491,29 @@ void MarshConnector::handle_heartbeat(mavlink_message_t message) { } } +Error MarshConnector::subscribe_message(uint8_t manager_system, + uint8_t manager_component, + uint32_t msgid) { + // subscribe to messages not sent to visualisation node by default + mavlink_command_long_t command; + command.target_system = manager_system; + command.target_component = manager_component; + command.command = MAV_CMD_SET_MESSAGE_INTERVAL; + command.confirmation = 0; + command.param1 = static_cast(msgid); + command.param2 = 0; // Default rate + command.param3 = 0; // Not used + command.param4 = 0; + command.param5 = 0; + command.param6 = 0; + command.param7 = 1; // Address of requester + + mavlink_message_t message_sent; + mavlink_msg_command_long_encode_chan(system_id, component_id, MAVLINK_COMM_0, + &message_sent, &command); + return send_message(message_sent); +} + void MarshConnector::manager_timeout() { print_line("Lost connection to MARSH Manager"); manager_connected = false; diff --git a/src/marshconnector.h b/src/marshconnector.h index 897300a..52ae44a 100644 --- a/src/marshconnector.h +++ b/src/marshconnector.h @@ -45,6 +45,10 @@ public: float get_collective(); // Get normalized (-1 to 1) pedals position, positive turn right float get_pedals(); + // Get normalized position for all controls + Vector4 get_controls(); + // Get normalized trim position + Vector4 get_trim(); // Is there a connection to MARSH Manager bool get_manager_connected(); @@ -76,6 +80,7 @@ private: void handle_attitude(mavlink_message_t message); void handle_param(mavlink_message_t message); void handle_manual_control(mavlink_message_t message); + void handle_manual_setpoint(mavlink_message_t message); void handle_heartbeat(mavlink_message_t message); void receive_model_data(); @@ -103,14 +108,18 @@ private: "LOCAL_FRAME_LAT", "LOCAL_FRAME_LON", }; Error send_param(Parameter index); + Error subscribe_message(uint8_t manager_system, uint8_t manager_component, + uint32_t msgid); uint8_t system_id; uint8_t component_id; // TODO: Interpolate with some delay + // Or not, in practice the simple solution works nicely Vector3 last_location; Quaternion last_rotation; Vector4 last_controls; + Vector4 last_trim; double time_passed; Timer *heartbeat_timer;