From b868c2bdbbce676cb7c2b49d54fab2d6c1388d19 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Marek=20S=2E=20=C5=81ukasiewicz?= <marek@lukasiewicz.tech>
Date: Wed, 12 Feb 2025 15:57:45 +0100
Subject: [PATCH] Cleanup MarshConnector

Receive ATTITUDE and LOCAL_POSITION_NED
Remove unnecessary prints
---
 project/main-xr.tscn   |  8 ++------
 project/main.tscn      |  1 -
 src/marshconnector.cpp | 27 +++++++++++++++++----------
 3 files changed, 19 insertions(+), 17 deletions(-)

diff --git a/project/main-xr.tscn b/project/main-xr.tscn
index 38d8ce1..18c3f25 100644
--- a/project/main-xr.tscn
+++ b/project/main-xr.tscn
@@ -22,7 +22,6 @@ sky = SubResource("Sky_0xm2m")
 transform = Transform3D(-1, 0, -8.74228e-08, 0, 1, 0, 8.74228e-08, 0, -1, 0, 0, -3)
 
 [node name="MarshConnector" type="MarshConnector" parent="." index="1"]
-process_mode = 0
 
 [node name="Aircraft" type="Node3D" parent="." index="2" node_paths=PackedStringArray("connector")]
 script = ExtResource("1_hips3")
@@ -31,12 +30,9 @@ connector = NodePath("../MarshConnector")
 [node name="MeshInstance3D" type="MeshInstance3D" parent="Aircraft" index="0"]
 mesh = SubResource("BoxMesh_2w36v")
 
-[node name="Camera3D" type="Camera3D" parent="." index="3"]
-transform = Transform3D(-1, -2.26267e-08, 8.44439e-08, 0, 0.965926, 0.258819, -8.74228e-08, 0.258819, -0.965926, 0, 1, -3)
-
-[node name="DirectionalLight3D" type="DirectionalLight3D" parent="." index="4"]
+[node name="DirectionalLight3D" type="DirectionalLight3D" parent="." index="3"]
 transform = Transform3D(-0.707107, -0.683013, 0.183013, 0, 0.258819, 0.965926, -0.707107, 0.683013, -0.183013, 3, 3, 3)
 
-[node name="WorldEnvironment" type="WorldEnvironment" parent="." index="5"]
+[node name="WorldEnvironment" type="WorldEnvironment" parent="." index="4"]
 environment = SubResource("Environment_jdgxj")
 camera_attributes = SubResource("CameraAttributesPractical_fpvso")
diff --git a/project/main.tscn b/project/main.tscn
index 6163934..2cb87ad 100644
--- a/project/main.tscn
+++ b/project/main.tscn
@@ -18,7 +18,6 @@ sky = SubResource("Sky_0xm2m")
 [node name="Main" type="Node3D"]
 
 [node name="MarshConnector" type="MarshConnector" parent="."]
-process_mode = 0
 
 [node name="Aircraft" type="Node3D" parent="." node_paths=PackedStringArray("connector")]
 script = ExtResource("1_ig7tw")
diff --git a/src/marshconnector.cpp b/src/marshconnector.cpp
index f11e62e..3a1990c 100644
--- a/src/marshconnector.cpp
+++ b/src/marshconnector.cpp
@@ -38,11 +38,6 @@ MarshConnector::MarshConnector() {
   heartbeat_timer->connect("timeout", Callable(this, "send_heartbeat"));
 
   socket = memnew(PacketPeerUDP);
-
-  if (Engine::get_singleton()->is_editor_hint()) {
-    // Don't run _process() in the editor
-    set_process_mode(Node::ProcessMode::PROCESS_MODE_DISABLED);
-  }
 }
 
 MarshConnector::~MarshConnector() {
@@ -65,7 +60,7 @@ void MarshConnector::_process(double delta) {
 }
 
 Error MarshConnector::send_heartbeat() {
-  print_line("Sending HEARTBEAT at ", time_passed, " seconds");
+  // print_line("Sending HEARTBEAT at ", time_passed, " seconds");
 
   mavlink_heartbeat_t heartbeat;
   heartbeat.type = MAV_TYPE_HELICOPTER;
@@ -84,10 +79,10 @@ Error MarshConnector::send_heartbeat() {
   for (int i = 0; i < sizeof(send_buffer); i++) {
     array.append(send_buffer[i]);
   }
-  print_line("Data array ", array);
+  // print_line("Data array ", array);
 
   const auto result = socket->put_packet(array);
-  print_line("Send result ", result);
+  // print_line("Send result ", result);
   return result;
 }
 
@@ -195,11 +190,23 @@ Quaternion MarshConnector::godot_rotation_from_mavlink(float roll_rad,
 }
 
 void MarshConnector::handle_local_position(mavlink_message_t message) {
-  print_line("Implement handle_local_position");
+  if (message.msgid != MAVLINK_MSG_ID_LOCAL_POSITION_NED)
+    return;
+
+  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);
 }
 
 void MarshConnector::handle_attitude(mavlink_message_t message) {
-  print_line("Implement handle_attitude");
+  if (message.msgid != MAVLINK_MSG_ID_ATTITUDE)
+    return;
+
+  mavlink_attitude_t attitude;
+  mavlink_msg_attitude_decode(&message, &attitude);
+  last_rotation =
+      godot_rotation_from_mavlink(attitude.roll, attitude.pitch, attitude.yaw);
 }
 
 void MarshConnector::handle_param(mavlink_message_t message) {