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");
 }