Recreate lidia controls with Node2D

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():

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"]

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(
+# Propagate the calls to child
-		"http://{}:{}/{}".format([lidia_hostname, lidia_port, CONTROLS_PATH], "{}"),
+func set_controls(current: Vector4):
-		$SubViewport2/TextureRect,
+	controls.call("set_controls", current)
-		{ "frame_rate": 90, "javascript": true },
+func set_trim(trim: Vector4):
-	)
+	controls.call("set_trim", trim)
-	browser_ctrl.name = "controls"
-	browser_ctrl.enable_ad_block(false) # Required for lidia static assets

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

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

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

project/instruments/controls.gd.uid

@@ -0,0 +1 @@
+uid://ds6f8cur05ka0

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")

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

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

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"]
+[node name="Controls" parent="SubViewport2" instance=ExtResource("2_372d7")]
-offset_right = 512.0
-offset_bottom = 512.0
-expand_mode = 5
 
 [node name="Quad2" type="MeshInstance3D" parent="."]
 transform = Transform3D(0.6, 0, 0, 0, 0.6, 0, 0, 0, 0.6, 0.425, 0, 0)

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

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

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

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"
@@ -26,6 +27,7 @@
 #include "mavlink/mavlink_types.h"
 #include <cstdint>
 #include <optional>
+#include <string.h>
 
 using namespace godot;
 
@@ -36,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"),
@@ -181,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;
@@ -243,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; }
@@ -403,10 +414,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;
@@ -435,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;
@@ -445,27 +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
+      Error result = subscribe_message(message.sysid, message.compid,
-      mavlink_command_long_t command;
+                                       MAVLINK_MSG_ID_MANUAL_CONTROL);
-      command.target_system = 1;
-      command.target_component =
-          MAV_COMP_ID_USER1; // HACK: Should get it from received HEARTBEAT
-      command.command = MAV_CMD_SET_MESSAGE_INTERVAL;
-      command.confirmation = 0;
-      command.param1 = static_cast<float>(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 requestor
-
-      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);
       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);
       }
     }
 
@@ -474,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<float>(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;

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;