Spin main rotor when receiving data from flight model›

project/aircraft/aircraft.gd

@@ -1,19 +1,45 @@
 extends Node3D
 
-@onready var connector = $MarshConnector
+## Rotation of cyclic stick model for full control deflection, in degrees
-@onready var skeleton = $"Mi-2/Armature/Skeleton3D"
+@export var range_cyclic: float = 30
-@onready var attitude_root = $AttitudeRoot
+
+## Main rotor speed, in revolutions per minute
+@export var rotor_rpm: float = 500
+
+## Current angle of rotor rotation
+var _rotor_azimuth: float = 0
+
+@onready var connector: MarshConnector = $MarshConnector
+@onready var skeleton: Skeleton3D = $"Mi-2/Armature/Skeleton3D"
+@onready var attitude_root: Node3D = $AttitudeRoot
 
 @onready var bone_cg: int = skeleton.find_bone("BodyCG")
+@onready var bone_cyclic: int = skeleton.find_bone("Cyclic")
+@onready var bone_rotor: int = skeleton.find_bone("Rotor")
 
-func _process(_delta: float) -> void:
+func _process(delta: float) -> void:
-	var target: Transform3D = connector.get_aircraft()
+	var target := connector.get_aircraft()
 	position = target.origin
 	
-	# Add the rotation to the bone
+	# Add the rotation to the correct bone for rotation around CG
-	var rest = skeleton.get_bone_rest(bone_cg).basis.get_rotation_quaternion()
+	var rest := skeleton.get_bone_rest(bone_cg).basis.get_rotation_quaternion()
-	var attitude = target.basis.get_rotation_quaternion()
+	var attitude := target.basis.get_rotation_quaternion()
 	skeleton.set_bone_pose_rotation(bone_cg, attitude * rest)
 
 	# Rotate other children (not using BoneAttachment3D due to jitter)
 	attitude_root.rotation = target.basis.get_euler()
+	
+	# Rotate the cyclic stick bone
+	var cyclic := connector.get_cyclic()
+	var cyc_rest := skeleton.get_bone_rest(bone_cyclic).basis.get_rotation_quaternion()
+	var cyc_angles_rad := 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)
+	
+	# Spin the rotor only when receiving flight data
+	if connector.get_model_connected():
+		_rotor_azimuth += rotor_rpm * 2 * PI / 60 * delta
+		var rotor_rest := skeleton.get_bone_rest(bone_rotor).basis.get_rotation_quaternion()
+		var rotor_att := Quaternion.from_euler(Vector3(0, _rotor_azimuth, 0))
+		# Note that this is reverse order, to rotate in local bone axes
+		skeleton.set_bone_pose_rotation(bone_rotor, rotor_rest * rotor_att)

project/aircraft/aircraft.tscn

@@ -16,10 +16,13 @@ material_override = SubResource("StandardMaterial3D_mrxe8")
 [node name="AttitudeRoot" type="Node3D" parent="."]
 transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 2.4, 0)
 
-[node name="XROrigin3D" type="XROrigin3D" parent="AttitudeRoot"]
+[node name="PilotEyes" type="Node3D" parent="AttitudeRoot"]
-transform = Transform3D(-1, 0, -8.74227e-08, 0, 0.999999, 0, 8.74228e-08, 0, -0.999999, 0.214, -0.721897, 1.53478)
+editor_description = "Added only to provide a convenient point to reset XROrigin3D to"
+transform = Transform3D(-1, 0, -8.74228e-08, 0, 1, 0, 8.74228e-08, 0, -1, 0.214, -0.721897, 1.53478)
 
-[node name="XRCamera3D" type="XRCamera3D" parent="AttitudeRoot/XROrigin3D"]
+[node name="XROrigin3D" type="XROrigin3D" parent="AttitudeRoot/PilotEyes"]
+
+[node name="XRCamera3D" type="XRCamera3D" parent="AttitudeRoot/PilotEyes/XROrigin3D"]
 
 [node name="MarshConnector" type="MarshConnector" parent="."]
 

project/main.tscn

@@ -22,6 +22,5 @@ transform = Transform3D(0.707107, 0.683013, -0.183013, 0, 0.258819, 0.965926, 0.
 
 [node name="StartVR" type="Node3D" parent="."]
 script = ExtResource("1_ig7tw")
-maximum_refresh_rate = 144
 
 [node name="Aircraft" parent="." instance=ExtResource("2_0xm2m")]

src/marshconnector.cpp

@@ -12,6 +12,7 @@
 #include "godot_cpp/variant/quaternion.hpp"
 #include "godot_cpp/variant/transform3d.hpp"
 #include "godot_cpp/variant/vector3.hpp"
+#include "godot_cpp/variant/vector4.hpp"
 #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"
@@ -28,14 +29,18 @@ 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_model_connected"),
+                       &MarshConnector::get_model_connected);
+  ClassDB::bind_method(D_METHOD("get_manager_connected"),
+                       &MarshConnector::get_manager_connected);
 
   // Timer callbacks
   ClassDB::bind_method(D_METHOD("send_heartbeat"),
                        &MarshConnector::send_heartbeat);
   ClassDB::bind_method(D_METHOD("manager_timeout"),
                        &MarshConnector::manager_timeout);
-  // ClassDB::bind_method(D_METHOD("model_timeout"),
+  ClassDB::bind_method(D_METHOD("model_timeout"),
-  //                      &MarshConnector::model_timeout);
+                       &MarshConnector::model_timeout);
 }
 
 MarshConnector::MarshConnector() {
@@ -59,6 +64,15 @@ MarshConnector::MarshConnector() {
   manager_timer->set_one_shot(true);
   manager_timer->set_autostart(false);
   manager_timer->connect("timeout", Callable(this, "manager_timeout"));
+
+  model_connected = false;
+
+  model_timer = memnew(Timer);
+  add_child(model_timer);
+  model_timer->set_wait_time(1.0);
+  model_timer->set_one_shot(true);
+  model_timer->set_autostart(false);
+  model_timer->connect("timeout", Callable(this, "model_timeout"));
 }
 
 MarshConnector::~MarshConnector() {
@@ -145,6 +159,30 @@ void MarshConnector::receive_data(const PackedByteArray &data) {
   }
 }
 
+void MarshConnector::receive_model_data() {
+  if (!model_connected) {
+    print_line("Started receiving flight model data");
+  }
+
+  model_connected = true;
+  model_timer->start();
+}
+
+void MarshConnector::model_timeout() {
+  print_line("Stopped receiving flight model data");
+  model_connected = false;
+}
+
+void MarshConnector::receive_location(Vector3 location) {
+  last_location = location;
+  receive_model_data();
+}
+
+void MarshConnector::receive_rotation(Quaternion rotation) {
+  last_rotation = rotation;
+  receive_model_data();
+}
+
 Error MarshConnector::set_parameter(const String &id, float value) {
   print_line("Implement set_parameter");
   return ERR_METHOD_NOT_FOUND;
@@ -164,8 +202,13 @@ Vector2 MarshConnector::get_cyclic() {
 }
 
 float MarshConnector::get_collective() { return last_controls.w; }
+
 float MarshConnector::get_pedals() { return last_controls.z; }
 
+bool MarshConnector::get_manager_connected() { return manager_connected; }
+
+bool MarshConnector::get_model_connected() { return model_connected; }
+
 Vector2 MarshConnector::local_meters_from_global_degrees(double latitude,
                                                          double longitude) {
 
@@ -197,10 +240,10 @@ void MarshConnector::handle_sim_state(mavlink_message_t message) {
 
   const Vector2 local_position = local_meters_from_global_degrees(lat, lon);
 
-  last_location = godot_location_from_mavlink(local_position.x,
+  receive_location(godot_location_from_mavlink(
-                                              local_position.y, sim_state.alt);
+      local_position.x, local_position.y, sim_state.alt));
-  last_rotation = godot_rotation_from_mavlink(sim_state.roll, sim_state.pitch,
+  receive_rotation(godot_rotation_from_mavlink(sim_state.roll, sim_state.pitch,
-                                              sim_state.yaw);
+                                               sim_state.yaw));
 }
 Vector3 MarshConnector::godot_location_from_mavlink(float north_meters,
                                                     float east_meters,
@@ -231,8 +274,8 @@ void MarshConnector::handle_local_position(mavlink_message_t message) {
 
   mavlink_local_position_ned_t local_pos;
   mavlink_msg_local_position_ned_decode(&message, &local_pos);
-  last_location =
+  receive_location(
-      godot_location_from_mavlink(local_pos.x, local_pos.y, -local_pos.z);
+      godot_location_from_mavlink(local_pos.x, local_pos.y, -local_pos.z));
 }
 
 void MarshConnector::handle_attitude(mavlink_message_t message) {
@@ -241,8 +284,8 @@ void MarshConnector::handle_attitude(mavlink_message_t message) {
 
   mavlink_attitude_t attitude;
   mavlink_msg_attitude_decode(&message, &attitude);
-  last_rotation =
+  receive_rotation(
-      godot_rotation_from_mavlink(attitude.roll, attitude.pitch, attitude.yaw);
+      godot_rotation_from_mavlink(attitude.roll, attitude.pitch, attitude.yaw));
 }
 
 void MarshConnector::handle_param(mavlink_message_t message) {

src/marshconnector.h

@@ -50,8 +50,8 @@ public:
 
   // Called by timer
   void manager_timeout();
-  // // Called by timer
+  // Called by timer
-  // void model_timeout();
+  void model_timeout();
 
   // Convert from global coordinates to local position x=north, y=east
   // taking into account current configuration.
@@ -75,6 +75,10 @@ private:
   void handle_manual_control(mavlink_message_t message);
   void handle_heartbeat(mavlink_message_t message);
 
+  void receive_model_data();
+  void receive_location(Vector3 location);
+  void receive_rotation(Quaternion rotation);
+
   Error send_message(mavlink_message_t message);
 
   // Do not specify specific values, to ensure PARAM_COUNT is correct
@@ -107,6 +111,8 @@ private:
 
   bool manager_connected;
   Timer *manager_timer;
+  bool model_connected;
+  Timer *model_timer;
 };
 
 } // namespace godot