visualisation-marsh/src/marshconnector.cpp

#include "marshconnector.h"

#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"
#include "godot_cpp/core/print_string.hpp"
#include "godot_cpp/variant/basis.hpp"
#include "godot_cpp/variant/packed_byte_array.hpp"
#include "godot_cpp/variant/quaternion.hpp"
#include "godot_cpp/variant/transform3d.hpp"
#include "godot_cpp/variant/vector3.hpp"
#include "godot_cpp/variant/vector4.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"

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);
  ClassDB::bind_method(D_METHOD("get_model_connected"),
                       &MarshConnector::get_model_connected);
  ClassDB::bind_method(D_METHOD("get_manager_connected"),
                       &MarshConnector::get_manager_connected);

  // Timer callbacks
  ClassDB::bind_method(D_METHOD("send_heartbeat"),
                       &MarshConnector::send_heartbeat);
  ClassDB::bind_method(D_METHOD("manager_timeout"),
                       &MarshConnector::manager_timeout);
  ClassDB::bind_method(D_METHOD("model_timeout"),
                       &MarshConnector::model_timeout);
}

MarshConnector::MarshConnector() {
  // Initialize member variables
  time_passed = 0.0;

  heartbeat_timer = memnew(Timer);
  add_child(heartbeat_timer);
  heartbeat_timer->set_wait_time(1.0);
  heartbeat_timer->set_one_shot(false);
  heartbeat_timer->set_autostart(true);
  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"));

  model_connected = false;

  model_timer = memnew(Timer);
  add_child(model_timer);
  model_timer->set_wait_time(1.0);
  model_timer->set_one_shot(true);
  model_timer->set_autostart(false);
  model_timer->connect("timeout", Callable(this, "model_timeout"));
}

MarshConnector::~MarshConnector() {
  // Free only manually managed member variables
}

void MarshConnector::_ready() { print_line("MarshConnector ready"); }

void MarshConnector::_process(double delta) {
  time_passed += delta;

  if (!socket->is_socket_connected()) {
    socket->connect_to_host("127.0.0.1", 24400);
  } else {
    while (socket->get_available_packet_count() > 0) {
      const PackedByteArray data = socket->get_packet();
      receive_data(data);
    }
  }
}

Error MarshConnector::send_heartbeat() {
  // print_line("Sending HEARTBEAT at ", time_passed, " seconds");

  mavlink_heartbeat_t heartbeat;
  heartbeat.type = MAV_TYPE_HELICOPTER;
  heartbeat.autopilot = MAV_AUTOPILOT_INVALID;
  heartbeat.base_mode = 0;   // none of the flags applicable
  heartbeat.custom_mode = 0; // not used so far
  heartbeat.system_status = MAV_STATE_ACTIVE;

  mavlink_message_t message;
  mavlink_msg_heartbeat_encode_chan(1, MARSH_COMP_ID_VISUALISATION,
                                    MAVLINK_COMM_0, &message, &heartbeat);
  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]);
  }

  const auto result = socket->put_packet(array);
  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;
      case MAVLINK_MSG_ID_MANUAL_CONTROL:
        handle_manual_control(message);
        break;
      case MAVLINK_MSG_ID_HEARTBEAT:
        handle_heartbeat(message);
        break;
      default:
        break;
      }
    }
  }
}

void MarshConnector::receive_model_data() {
  if (!model_connected) {
    print_line("Started receiving flight model data");
  }

  model_connected = true;
  model_timer->start();
}

void MarshConnector::model_timeout() {
  print_line("Stopped receiving flight model data");
  model_connected = false;
}

void MarshConnector::receive_location(Vector3 location) {
  last_location = location;
  receive_model_data();
}

void MarshConnector::receive_rotation(Quaternion rotation) {
  last_rotation = rotation;
  receive_model_data();
}

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

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

bool MarshConnector::get_manager_connected() { return manager_connected; }

bool MarshConnector::get_model_connected() { return model_connected; }

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] * Math_PI / 180.0;
  const double lon0 = parameters[LOCAL_FRAME_LON] * Math_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);

  receive_location(godot_location_from_mavlink(
      local_position.x, local_position.y, sim_state.alt));
  receive_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) {
  if (message.msgid != MAVLINK_MSG_ID_LOCAL_POSITION_NED)
    return;

  mavlink_local_position_ned_t local_pos;
  mavlink_msg_local_position_ned_decode(&message, &local_pos);
  receive_location(
      godot_location_from_mavlink(local_pos.x, local_pos.y, -local_pos.z));
}

void MarshConnector::handle_attitude(mavlink_message_t message) {
  if (message.msgid != MAVLINK_MSG_ID_ATTITUDE)
    return;

  mavlink_attitude_t attitude;
  mavlink_msg_attitude_decode(&message, &attitude);
  receive_rotation(
      godot_rotation_from_mavlink(attitude.roll, attitude.pitch, attitude.yaw));
}

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