WIP: Receive MAVLink messages

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

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