WIP: Receive MAVLink messages

Untested conversion from SIM_STATE.
Defined API for parameters, but functions are unimplemented

src/marshconnector.cpp

@@ -3,10 +3,14 @@
 #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/math.hpp"
 #include "godot_cpp/core/memory.hpp"
 #include "godot_cpp/core/print_string.hpp"
 #include "godot_cpp/variant/packed_byte_array.hpp"
+#include "godot_cpp/variant/quaternion.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_sim_state.h"
 #include "mavlink/mavlink_helpers.h"
 
 // How much extra bytes are needed on top of message payload length
@@ -52,7 +56,7 @@ void MarshConnector::_process(double delta) {
   }
 }
 
-void MarshConnector::send_heartbeat() {
+Error MarshConnector::send_heartbeat() {
   print_line("Sending HEARTBEAT at ", time_passed, " seconds");
 
   mavlink_heartbeat_t heartbeat;
@@ -76,4 +80,116 @@ void MarshConnector::send_heartbeat() {
 
   const auto result = socket->put_packet(array);
   print_line("Send result ", result);
+  return result;
+}
+
+void MarshConnector::receive_data(const PackedByteArray &data) {
+  mavlink_message_t message;
+  mavlink_status_t parser_status;
+
+  for (size_t i = 0; i < data.size(); i++) {
+
+    if (mavlink_parse_char(MAVLINK_COMM_0, data.ptr()[i], &message,
+                           &parser_status)) {
+      switch (message.msgid) {
+      case MAVLINK_MSG_ID_SIM_STATE:
+        handle_sim_state(message);
+        break;
+      case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
+        handle_local_position(message);
+        break;
+      case MAVLINK_MSG_ID_ATTITUDE:
+        handle_attitude(message);
+        break;
+      case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
+      case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
+      case MAVLINK_MSG_ID_PARAM_SET:
+        handle_param(message);
+        break;
+      default:
+        break;
+      }
+    }
+  }
+}
+
+Error MarshConnector::set_parameter(const String &id, float value) {
+  print_line("Implement set_parameter");
+  return ERR_METHOD_NOT_FOUND;
+}
+
+float MarshConnector::get_parameter(const String &id) {
+  print_line("Implement get_parameter");
+  return Math_NAN;
+}
+
+Vector2 MarshConnector::local_meters_from_global_degrees(double latitude,
+                                                         double longitude) {
+
+  // Convert everything to radians
+  const double lat = latitude * Math_PI / 180.0;
+  const double lon = longitude * Math_PI / 180.0;
+  const double lat0 = parameters[LOCAL_FRAME_LAT] * M_PI / 180.0;
+  const double lon0 = parameters[LOCAL_FRAME_LON] * M_PI / 180.0;
+
+  double EARTH_RADIUS = 6371008.7714; // mean radius for WGS-84, in meters
+
+  // X North is just length along the meridian
+  double x = (lat - lat0) * EARTH_RADIUS;
+  // Y East is just length along the circle of latitude
+  double y = (lon - lon0) * (EARTH_RADIUS * cos(lat0));
+
+  return Vector2(x, y);
+}
+void MarshConnector::handle_sim_state(mavlink_message_t message) {
+  if (message.msgid != MAVLINK_MSG_ID_SIM_STATE)
+    return;
+
+  mavlink_sim_state_t sim_state;
+  mavlink_msg_sim_state_decode(&message, &sim_state);
+  const double lat =
+      sim_state.lat_int != 0 ? sim_state.lat_int / 1.0e7 : sim_state.lat;
+  const double lon =
+      sim_state.lon_int != 0 ? sim_state.lon_int / 1.0e7 : sim_state.lon;
+
+  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);
+}
+Vector3 MarshConnector::godot_location_from_mavlink(float north_meters,
+                                                    float east_meters,
+                                                    float alt_meters) {
+  // See Godot documentation for axis conventions:
+  // https://docs.godotengine.org/en/stable/classes/class_vector3.html#constants
+  return Vector3{
+      east_meters,  // East is RIGHT = Vector3(1, 0, 0)
+      alt_meters,   // Up is UP = Vector3(0, 1, 0)
+      north_meters, // North is FORWARD = Vector3(0, 0, -1)
+  };
+}
+Quaternion MarshConnector::godot_rotation_from_mavlink(float roll_rad,
+                                                       float pitch_rad,
+                                                       float yaw_rad) {
+  // Checked manually in Godot editor
+  const Vector3 euler = Vector3{
+      -pitch_rad,
+      -yaw_rad,
+      roll_rad,
+  };
+  return Quaternion::from_euler(euler);
+}
+
+void MarshConnector::handle_local_position(mavlink_message_t message) {
+  print_line("Implement handle_local_position");
+}
+
+void MarshConnector::handle_attitude(mavlink_message_t message) {
+  print_line("Implement handle_attitude");
+}
+
+void MarshConnector::handle_param(mavlink_message_t message) {
+  print_line("Implement handle_param");
 }

src/marshconnector.h

@@ -4,17 +4,16 @@
 #include "godot_cpp/classes/node.hpp"
 #include "godot_cpp/classes/packet_peer_udp.hpp"
 #include "godot_cpp/classes/timer.hpp"
+#include "godot_cpp/variant/quaternion.hpp"
+#include "godot_cpp/variant/vector2.hpp"
+#include "godot_cpp/variant/vector3.hpp"
+#include "mavlink/mavlink_types.h"
 
 namespace godot {
 
 class MarshConnector : public Node {
   GDCLASS(MarshConnector, Node)
 
-private:
-  double time_passed;
-  Timer *heartbeat_timer;
-  PacketPeerUDP *socket;
-
 protected:
   static void _bind_methods();
 
@@ -25,7 +24,61 @@ public:
   void _ready() override;
   void _process(double delta) override;
 
-  void send_heartbeat();
+  // Returns the error from put_packet
+  Error send_heartbeat();
+  // Handle MAVLink messages fully contained in data
+  void receive_data(const PackedByteArray &data);
+  // Set parameter value with some validation
+  Error set_parameter(const String &id, float value);
+  // Get current parameter value
+  float get_parameter(const String &id);
+
+  // Convert from global coordinates to local position x=north, y=east
+  // taking into account current configuration.
+  // Note that 32-bit float type is not exact enough to store global position
+  Vector2 local_meters_from_global_degrees(double latitude, double longitude);
+
+  static Vector3 godot_location_from_mavlink(float north_meters,
+                                             float east_meters,
+                                             float alt_meters);
+  static Quaternion godot_rotation_from_mavlink(float roll_rad, float pitch_rad,
+                                                float yaw_rad);
+
+private:
+  // Handle corresponding MAVLink message received.
+  // The functions ensure the type is correct; message_t is used, only
+  // to keep the amount of code required in the header small.
+  void handle_sim_state(mavlink_message_t message);
+  void handle_local_position(mavlink_message_t message);
+  void handle_attitude(mavlink_message_t message);
+  void handle_param(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
+  // inside the class namespace anyway
+  enum Parameter {
+    // FOG_DENSITY,
+    NAV_OFS_HDG,
+    NAV_OFS_X,
+    NAV_OFS_Y,
+    LOCAL_FRAME_LAT,
+    LOCAL_FRAME_LON,
+    PARAM_COUNT,
+  };
+  float parameters[PARAM_COUNT];
+  const String parameter_names[PARAM_COUNT] = {
+      // "FOG_DENSITY",
+      "NAV_OFS_HDG",     "NAV_OFS_X",       "NAV_OFS_Y",
+      "LOCAL_FRAME_LAT", "LOCAL_FRAME_LON",
+  };
+
+  // TODO: Interpolate with some delay
+  Vector3 last_location;
+  Quaternion last_rotation;
+
+  double time_passed;
+  Timer *heartbeat_timer;
+  PacketPeerUDP *socket;
 };
 
 } // namespace godot