diff --git a/project/aircraft/aircraft.gd b/project/aircraft/aircraft.gd index f8c9918..3ab651d 100644 --- a/project/aircraft/aircraft.gd +++ b/project/aircraft/aircraft.gd @@ -1,19 +1,45 @@ extends Node3D -@onready var connector = $MarshConnector -@onready var skeleton = $"Mi-2/Armature/Skeleton3D" -@onready var attitude_root = $AttitudeRoot +## Rotation of cyclic stick model for full control deflection, in degrees +@export var range_cyclic: float = 30 + +## 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: - var target: Transform3D = connector.get_aircraft() +func _process(delta: float) -> void: + var target := connector.get_aircraft() position = target.origin - # Add the rotation to the bone - var rest = skeleton.get_bone_rest(bone_cg).basis.get_rotation_quaternion() - var attitude = target.basis.get_rotation_quaternion() + # Add the rotation to the correct bone for rotation around CG + var rest := skeleton.get_bone_rest(bone_cg).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) diff --git a/project/aircraft/aircraft.tscn b/project/aircraft/aircraft.tscn index 867edc0..bc78d00 100644 --- a/project/aircraft/aircraft.tscn +++ b/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"] -transform = Transform3D(-1, 0, -8.74227e-08, 0, 0.999999, 0, 8.74228e-08, 0, -0.999999, 0.214, -0.721897, 1.53478) +[node name="PilotEyes" type="Node3D" parent="AttitudeRoot"] +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="."] diff --git a/project/main.tscn b/project/main.tscn index 9b40194..60db560 100644 --- a/project/main.tscn +++ b/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")] diff --git a/src/marshconnector.cpp b/src/marshconnector.cpp index 0d171a6..a0364a1 100644 --- a/src/marshconnector.cpp +++ b/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"), - // &MarshConnector::model_timeout); + ClassDB::bind_method(D_METHOD("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, - local_position.y, sim_state.alt); - last_rotation = godot_rotation_from_mavlink(sim_state.roll, sim_state.pitch, - sim_state.yaw); + receive_location(godot_location_from_mavlink( + local_position.x, local_position.y, sim_state.alt)); + receive_rotation(godot_rotation_from_mavlink(sim_state.roll, sim_state.pitch, + 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 = - godot_location_from_mavlink(local_pos.x, local_pos.y, -local_pos.z); + receive_location( + 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 = - godot_rotation_from_mavlink(attitude.roll, attitude.pitch, attitude.yaw); + receive_rotation( + godot_rotation_from_mavlink(attitude.roll, attitude.pitch, attitude.yaw)); } void MarshConnector::handle_param(mavlink_message_t message) { diff --git a/src/marshconnector.h b/src/marshconnector.h index cb7eb49..9e86eac 100644 --- a/src/marshconnector.h +++ b/src/marshconnector.h @@ -50,8 +50,8 @@ public: // Called by timer void manager_timeout(); - // // Called by timer - // void model_timeout(); + // Called by timer + 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