Send HEARTBEAT mesage from Godot

SConstruct

@@ -14,6 +14,7 @@ env = SConscript("modules/godot-cpp/SConstruct")
 
 # tweak this if you want to use different folders, or more folders, to store your source code in.
 env.Append(CPPPATH=["src/"])
+env.Append(CPPPATH=["include/"])
 sources = Glob("src/*.cpp")
 
 # Generated with local Godot installation
@@ -48,5 +49,7 @@ else:
         "project/bin/libmarshconnector{}{}".format(env["suffix"], env["SHLIBSUFFIX"]),
         source=sources,
     )
+    # Triggered a lot in MAVLink headers
+    env.Append(CXXFLAGS=["-Wno-address-of-packed-member"])
 
 Default(library)

src/marshconnector.cpp

@@ -1,8 +1,16 @@
 #include "marshconnector.h"
 
 #include "godot_cpp/classes/engine.hpp"
+#include "godot_cpp/classes/font_file.hpp"
+#include "godot_cpp/classes/packet_peer_udp.hpp"
 #include "godot_cpp/core/memory.hpp"
 #include "godot_cpp/core/print_string.hpp"
+#include "godot_cpp/variant/packed_byte_array.hpp"
+#include "mavlink/all/mavlink.h" // IWYU pragma: keep; always include the mavlink.h file for selected dialect
+#include "mavlink/mavlink_helpers.h"
+
+// How much extra bytes are needed on top of message payload length
+#define MAVLINK_OVERHEAD 12
 
 using namespace godot;
 
@@ -22,6 +30,8 @@ MarshConnector::MarshConnector() {
   heartbeat_timer->set_autostart(true);
   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);
@@ -32,8 +42,38 @@ MarshConnector::~MarshConnector() {
   // Free only manually managed member variables
 }
 
-void MarshConnector::_process(double delta) { time_passed += delta; }
+void MarshConnector::_ready() { print_line("MarshConnector ready"); }
+
+void MarshConnector::_process(double delta) {
+  time_passed += delta;
+
+  if (!socket->is_socket_connected()) {
+    socket->connect_to_host("127.0.0.1", 24400);
+  }
+}
 
 void MarshConnector::send_heartbeat() {
   print_line("Sending HEARTBEAT at ", time_passed, " seconds");
+
+  mavlink_heartbeat_t heartbeat;
+  heartbeat.type = MAV_TYPE_HELICOPTER;
+  heartbeat.autopilot = MAV_AUTOPILOT_INVALID;
+  heartbeat.base_mode = 0;   // none of the flags applicable
+  heartbeat.custom_mode = 0; // not used so far
+  heartbeat.system_status = MAV_STATE_ACTIVE;
+
+  mavlink_message_t message;
+  mavlink_msg_heartbeat_encode_chan(1, MARSH_COMP_ID_VISUALISATION,
+                                    MAVLINK_COMM_0, &message, &heartbeat);
+  uint8_t send_buffer[MAVLINK_OVERHEAD + MAVLINK_MSG_ID_HEARTBEAT_LEN];
+  mavlink_msg_to_send_buffer(send_buffer, &message);
+
+  PackedByteArray array;
+  for (int i = 0; i < sizeof(send_buffer); i++) {
+    array.append(send_buffer[i]);
+  }
+  print_line("Data array ", array);
+
+  const auto result = socket->put_packet(array);
+  print_line("Send result ", result);
 }

src/marshconnector.h

@@ -2,6 +2,7 @@
 #define MARSHCONNECTOR_H
 
 #include "godot_cpp/classes/node.hpp"
+#include "godot_cpp/classes/packet_peer_udp.hpp"
 #include "godot_cpp/classes/timer.hpp"
 
 namespace godot {
@@ -12,6 +13,7 @@ class MarshConnector : public Node {
 private:
   double time_passed;
   Timer *heartbeat_timer;
+  PacketPeerUDP *socket;
 
 protected:
   static void _bind_methods();
@@ -20,6 +22,7 @@ public:
   MarshConnector();
   ~MarshConnector();
 
+  void _ready() override;
   void _process(double delta) override;
 
   void send_heartbeat();