120 lines
3.7 KiB
C++
120 lines
3.7 KiB
C++
#ifndef MARSHCONNECTOR_H
|
|
#define MARSHCONNECTOR_H
|
|
|
|
#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/transform3d.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)
|
|
|
|
protected:
|
|
static void _bind_methods();
|
|
|
|
public:
|
|
MarshConnector();
|
|
~MarshConnector();
|
|
|
|
void _ready() override;
|
|
void _process(double delta) override;
|
|
|
|
// 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);
|
|
|
|
// 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.
|
|
// 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);
|
|
void handle_manual_control(mavlink_message_t message);
|
|
void handle_heartbeat(mavlink_message_t message);
|
|
|
|
void receive_model_data();
|
|
void receive_location(Vector3 location);
|
|
void receive_rotation(Quaternion rotation);
|
|
|
|
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
|
|
// 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] = {0.0, 0.0, 0.0, 0.0, 0.0};
|
|
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;
|
|
Vector4 last_controls;
|
|
|
|
double time_passed;
|
|
Timer *heartbeat_timer;
|
|
PacketPeerUDP *socket;
|
|
|
|
bool manager_connected;
|
|
Timer *manager_timer;
|
|
bool model_connected;
|
|
Timer *model_timer;
|
|
};
|
|
|
|
} // namespace godot
|
|
|
|
#endif // MARSHCONNECTOR_H
|