diff --git a/project/aircraft/aircraft.gd b/project/aircraft/aircraft.gd index 3ab651d..f8c9918 100644 --- a/project/aircraft/aircraft.gd +++ b/project/aircraft/aircraft.gd @@ -1,45 +1,19 @@ extends Node3D -## 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 connector = $MarshConnector +@onready var skeleton = $"Mi-2/Armature/Skeleton3D" +@onready var attitude_root = $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 := connector.get_aircraft() +func _process(_delta: float) -> void: + var target: Transform3D = connector.get_aircraft() position = target.origin - # 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() + # 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() 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 bc78d00..867edc0 100644 --- a/project/aircraft/aircraft.tscn +++ b/project/aircraft/aircraft.tscn @@ -16,13 +16,10 @@ 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="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="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="XROrigin3D" type="XROrigin3D" parent="AttitudeRoot/PilotEyes"] - -[node name="XRCamera3D" type="XRCamera3D" parent="AttitudeRoot/PilotEyes/XROrigin3D"] +[node name="XRCamera3D" type="XRCamera3D" parent="AttitudeRoot/XROrigin3D"] [node name="MarshConnector" type="MarshConnector" parent="."] diff --git a/project/main.tscn b/project/main.tscn index 60db560..9b40194 100644 --- a/project/main.tscn +++ b/project/main.tscn @@ -22,5 +22,6 @@ 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 a0364a1..0d171a6 100644 --- a/src/marshconnector.cpp +++ b/src/marshconnector.cpp @@ -12,7 +12,6 @@ #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" @@ -29,18 +28,14 @@ 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() { @@ -64,15 +59,6 @@ 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() { @@ -159,30 +145,6 @@ 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; @@ -202,13 +164,8 @@ 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) { @@ -240,10 +197,10 @@ void MarshConnector::handle_sim_state(mavlink_message_t message) { const Vector2 local_position = local_meters_from_global_degrees(lat, lon); - 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)); + 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); } Vector3 MarshConnector::godot_location_from_mavlink(float north_meters, float east_meters, @@ -274,8 +231,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); - receive_location( - godot_location_from_mavlink(local_pos.x, local_pos.y, -local_pos.z)); + last_location = + godot_location_from_mavlink(local_pos.x, local_pos.y, -local_pos.z); } void MarshConnector::handle_attitude(mavlink_message_t message) { @@ -284,8 +241,8 @@ void MarshConnector::handle_attitude(mavlink_message_t message) { mavlink_attitude_t attitude; mavlink_msg_attitude_decode(&message, &attitude); - receive_rotation( - godot_rotation_from_mavlink(attitude.roll, attitude.pitch, attitude.yaw)); + last_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 9e86eac..cb7eb49 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,10 +75,6 @@ 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 @@ -111,8 +107,6 @@ private: bool manager_connected; Timer *manager_timer; - bool model_connected; - Timer *model_timer; }; } // namespace godot