Track connection to manager and receive MANUAL_CONTROL

src/marshconnector.cpp

@@ -2,6 +2,7 @@
 
 #include "godot_cpp/classes/engine.hpp"
 #include "godot_cpp/classes/font_file.hpp"
+#include "godot_cpp/classes/global_constants.hpp"
 #include "godot_cpp/classes/packet_peer_udp.hpp"
 #include "godot_cpp/core/math.hpp"
 #include "godot_cpp/core/memory.hpp"
@@ -12,18 +13,29 @@
 #include "godot_cpp/variant/transform3d.hpp"
 #include "godot_cpp/variant/vector3.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"
 #include "mavlink/common/mavlink_msg_sim_state.h"
 #include "mavlink/mavlink_helpers.h"
-
+#include "mavlink/mavlink_types.h"
-// How much extra bytes are needed on top of message payload length
-#define MAVLINK_OVERHEAD 12
 
 using namespace godot;
 
 void MarshConnector::_bind_methods() {
+  // Data access
+  ClassDB::bind_method(D_METHOD("get_aircraft"), &MarshConnector::get_aircraft);
+  ClassDB::bind_method(D_METHOD("get_cyclic"), &MarshConnector::get_cyclic);
+  ClassDB::bind_method(D_METHOD("get_collective"),
+                       &MarshConnector::get_collective);
+  ClassDB::bind_method(D_METHOD("get_pedals"), &MarshConnector::get_pedals);
+
+  // Timer callbacks
   ClassDB::bind_method(D_METHOD("send_heartbeat"),
                        &MarshConnector::send_heartbeat);
-  ClassDB::bind_method(D_METHOD("get_aircraft"), &MarshConnector::get_aircraft);
+  ClassDB::bind_method(D_METHOD("manager_timeout"),
+                       &MarshConnector::manager_timeout);
+  // ClassDB::bind_method(D_METHOD("model_timeout"),
+  //                      &MarshConnector::model_timeout);
 }
 
 MarshConnector::MarshConnector() {
@@ -38,6 +50,15 @@ MarshConnector::MarshConnector() {
   heartbeat_timer->connect("timeout", Callable(this, "send_heartbeat"));
 
   socket = memnew(PacketPeerUDP);
+
+  manager_connected = false;
+
+  manager_timer = memnew(Timer);
+  add_child(manager_timer);
+  manager_timer->set_wait_time(5.0);
+  manager_timer->set_one_shot(true);
+  manager_timer->set_autostart(false);
+  manager_timer->connect("timeout", Callable(this, "manager_timeout"));
 }
 
 MarshConnector::~MarshConnector() {
@@ -72,17 +93,19 @@ Error MarshConnector::send_heartbeat() {
   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];
+  return send_message(message);
+}
+
+Error MarshConnector::send_message(mavlink_message_t message) {
+  uint8_t send_buffer[MAVLINK_MAX_PACKET_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);
   return result;
 }
 
@@ -109,6 +132,12 @@ void MarshConnector::receive_data(const PackedByteArray &data) {
       case MAVLINK_MSG_ID_PARAM_SET:
         handle_param(message);
         break;
+      case MAVLINK_MSG_ID_MANUAL_CONTROL:
+        handle_manual_control(message);
+        break;
+      case MAVLINK_MSG_ID_HEARTBEAT:
+        handle_heartbeat(message);
+        break;
       default:
         break;
       }
@@ -130,6 +159,13 @@ Transform3D MarshConnector::get_aircraft() {
   return Transform3D{Basis{last_rotation}, last_location};
 }
 
+Vector2 MarshConnector::get_cyclic() {
+  return Vector2{last_controls.x, last_controls.y};
+}
+
+float MarshConnector::get_collective() { return last_controls.w; }
+float MarshConnector::get_pedals() { return last_controls.z; }
+
 Vector2 MarshConnector::local_meters_from_global_degrees(double latitude,
                                                          double longitude) {
 
@@ -212,3 +248,64 @@ void MarshConnector::handle_attitude(mavlink_message_t message) {
 void MarshConnector::handle_param(mavlink_message_t message) {
   print_line("Implement handle_param");
 }
+
+void MarshConnector::handle_manual_control(mavlink_message_t message) {
+  if (message.msgid != MAVLINK_MSG_ID_MANUAL_CONTROL)
+    return;
+
+  mavlink_manual_control_t manual_control;
+  mavlink_msg_manual_control_decode(&message, &manual_control);
+  const int16_t invalid = 0x7FFF;
+  if (manual_control.y != invalid)
+    last_controls.x = manual_control.y / 1000.0f;
+  if (manual_control.x != invalid)
+    last_controls.y = manual_control.x / -1000.0f;
+  if (manual_control.r != invalid)
+    last_controls.z = manual_control.r / 1000.0f;
+  if (manual_control.z != invalid)
+    last_controls.w = manual_control.z / 1000.0f;
+}
+
+void MarshConnector::handle_heartbeat(mavlink_message_t message) {
+  if (message.msgid != MAVLINK_MSG_ID_HEARTBEAT)
+    return;
+
+  // mavlink_heartbeat_t heartbeat;
+  // mavlink_msg_heartbeat_decode(&message, &heartbeat);
+  if (message.compid == MARSH_COMP_ID_MANAGER) {
+    if (!manager_connected) {
+      print_line("Connected to MARSH Manager");
+
+      // subscribe to messages not sent to visualisation node by default
+      mavlink_command_long_t command;
+      command.target_system = 1;
+      command.target_component = MARSH_COMP_ID_MANAGER;
+      command.command = MAV_CMD_SET_MESSAGE_INTERVAL;
+      command.confirmation = 0;
+      command.param1 = static_cast<float>(MAVLINK_MSG_ID_MANUAL_CONTROL);
+      command.param2 = 0; // Default rate
+      command.param3 = 0; // Not used
+      command.param4 = 0;
+      command.param5 = 0;
+      command.param6 = 0;
+      command.param7 = 1; // Address of requestor
+
+      mavlink_message_t message_sent;
+      mavlink_msg_command_long_encode_chan(1, MARSH_COMP_ID_VISUALISATION,
+                                           MAVLINK_COMM_0, &message_sent,
+                                           &command);
+      Error result = send_message(message_sent);
+      if (result != OK) {
+        print_line("Subscribe send result ", result);
+      }
+    }
+
+    manager_connected = true;
+    manager_timer->start();
+  }
+}
+
+void MarshConnector::manager_timeout() {
+  print_line("Lost connection to MARSH Manager");
+  manager_connected = false;
+}

src/marshconnector.h

@@ -33,8 +33,25 @@ public:
   Error set_parameter(const String &id, float value);
   // Get current parameter value
   float get_parameter(const String &id);
+
   // Get current state of the aircraft
   Transform3D get_aircraft();
+  // Get normalized (-1 to 1) cyclic position, where X is right, Y is pitch up
+  Vector2 get_cyclic();
+  // Get normalized (0 to 1) collective position, positive to climb
+  float get_collective();
+  // Get normalized (-1 to 1) pedals position, positive turn right
+  float get_pedals();
+
+  // Is there a connection to MARSH Manager
+  bool get_manager_connected();
+  // Is receiving data from flight model
+  bool get_model_connected();
+
+  // Called by timer
+  void manager_timeout();
+  // // Called by timer
+  // void model_timeout();
 
   // Convert from global coordinates to local position x=north, y=east
   // taking into account current configuration.
@@ -55,6 +72,10 @@ private:
   void handle_local_position(mavlink_message_t message);
   void handle_attitude(mavlink_message_t message);
   void handle_param(mavlink_message_t message);
+  void handle_manual_control(mavlink_message_t message);
+  void handle_heartbeat(mavlink_message_t message);
+
+  Error send_message(mavlink_message_t message);
 
   // Do not specify specific values, to ensure PARAM_COUNT is correct
   // Not an enum class on purpose, to make use more convenient, it's scoped
@@ -78,10 +99,14 @@ private:
   // TODO: Interpolate with some delay
   Vector3 last_location;
   Quaternion last_rotation;
+  Vector4 last_controls;
 
   double time_passed;
   Timer *heartbeat_timer;
   PacketPeerUDP *socket;
+
+  bool manager_connected;
+  Timer *manager_timer;
 };
 
 } // namespace godot