diff --git a/cmake/FindMAVLink.cmake b/cmake/FindMAVLink.cmake
new file mode 100644
index 0000000..f3b8329
--- /dev/null
+++ b/cmake/FindMAVLink.cmake
@@ -0,0 +1,21 @@
+include(FindPackageHandleStandardArgs)
+
+set(MAVLINK_SEARCH_PATHS
+ /usr
+ /usr/local
+ /opt/local
+)
+
+find_path(MAVLINK_INCLUDE_DIR
+ NAMES mavlink_types.h
+ PATHS
+ ${MAVLINK_SEARCH_PATHS}
+ PATH_SUFFIXES
+ include
+ include/mavlink
+ include/mavlink/v1.0
+)
+
+find_package_handle_standard_args(MAVLink DEFAULT_MSG MAVLINK_INCLUDE_DIR)
+
+mark_as_advanced(MAVLINK_INCLUDE_DIR)
diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt
index 148bd49..e6fbce6 100644
--- a/src/plugins/CMakeLists.txt
+++ b/src/plugins/CMakeLists.txt
@@ -1,11 +1,12 @@
if (ANDROID)
set(fileName ${CMAKE_BINARY_DIR}/kirogicore-android-dependencies.xml)
file(WRITE "${fileName}" "\n"
"\n"
"\n"
"\n")
install(FILES ${fileName} DESTINATION ${KDE_INSTALL_LIBDIR})
endif()
+add_subdirectory(mavlink)
add_subdirectory(parrot)
add_subdirectory(ryzetello)
diff --git a/src/plugins/mavlink/CMakeLists.txt b/src/plugins/mavlink/CMakeLists.txt
new file mode 100644
index 0000000..63f3bc0
--- /dev/null
+++ b/src/plugins/mavlink/CMakeLists.txt
@@ -0,0 +1,47 @@
+add_definitions(-DTRANSLATION_DOMAIN=\"kirogimavlinkplugin\")
+
+set(kirogimavlinkplugin_SRCS
+ mavlinkconnection.cpp
+ mavlinkplugin.cpp
+ mavlinkvehicle.cpp
+)
+
+ecm_qt_declare_logging_category(kirogimavlinkplugin_SRCS
+ HEADER debug.h
+ IDENTIFIER KIROGI_VEHICLESUPPORT_MAVLINK
+ CATEGORY_NAME "kirogi.vehiclesupport.mavlink"
+)
+
+kcoreaddons_add_plugin(kirogimavlinkplugin
+ SOURCES ${kirogimavlinkplugin_SRCS}
+ JSON "${CMAKE_CURRENT_SOURCE_DIR}/kirogimavlinkplugin.json"
+ INSTALL_NAMESPACE "kirogi/vehiclesupport"
+)
+
+find_package(MAVLink REQUIRED)
+
+include_directories(
+ ${MAVLINK_INCLUDE_DIR}
+ ${MAVLINK_INCLUDE_DIR}/ardupilotmega
+)
+
+set_target_properties(kirogimavlinkplugin
+ PROPERTIES
+ LIBRARY_OUTPUT_DIRECTORY "${CMAKE_LIBRARY_OUTPUT_DIRECTORY}"
+)
+
+target_link_libraries(kirogimavlinkplugin
+ PRIVATE
+ Qt5::Core
+ Qt5::Network
+ Qt5::Positioning
+ KF5::CoreAddons
+ KF5::I18n
+ KirogiCore
+)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ target_compile_options(kirogimavlinkplugin PRIVATE -pedantic)
+endif()
+
+install(FILES kirogimavlinkplugin.categories DESTINATION ${KDE_INSTALL_CONFDIR})
diff --git a/src/plugins/mavlink/Messages.sh b/src/plugins/mavlink/Messages.sh
new file mode 100755
index 0000000..02ef2fa
--- /dev/null
+++ b/src/plugins/mavlink/Messages.sh
@@ -0,0 +1,2 @@
+#! /bin/sh
+$XGETTEXT `find . -name \*.cpp` -o $podir/kirogimavlinkplugin.pot
diff --git a/src/plugins/mavlink/kirogimavlinkplugin.categories b/src/plugins/mavlink/kirogimavlinkplugin.categories
new file mode 100644
index 0000000..36576c4
--- /dev/null
+++ b/src/plugins/mavlink/kirogimavlinkplugin.categories
@@ -0,0 +1 @@
+kirogi.vehiclesupport.mavlink Kirogi MAVLink Vehicle Support Plugin DEFAULT_SEVERITY [WARNING] IDENTIFIER [KIROGI_VEHICLESUPPORT_MAVLINK]
diff --git a/src/plugins/mavlink/kirogimavlinkplugin.json b/src/plugins/mavlink/kirogimavlinkplugin.json
new file mode 100644
index 0000000..9fef9ec
--- /dev/null
+++ b/src/plugins/mavlink/kirogimavlinkplugin.json
@@ -0,0 +1,39 @@
+{
+ "KPlugin": {
+ "Authors": [
+ {
+ "Email": "patrickjp@kde.org",
+ "Name": "Patrick José Pereira",
+ "Name[ca]": "Patrick José Pereira",
+ "Name[cs]": "Patrick José Pereira",
+ "Name[es]": "Patrick José Pereira",
+ "Name[it]": "Patrick José Pereira",
+ "Name[nl]": "Patrick José Pereira",
+ "Name[pl]": "Patrick José Pereira",
+ "Name[pt]": "Patrick José Pereira",
+ "Name[sk]": "Patrick José Pereira",
+ "Name[sv]": "Patrick José Pereira",
+ "Name[uk]": "Patrick José Pereira",
+ "Name[x-test]": "xxPatrick José Pereiraxx"
+ }
+ ],
+ "Description": "A Kirogi plugin to support MAVLink vehicles",
+ "Description[pt]": "Um 'plugin' do Kirogi para suportar veiculos que utilizam MAVLink",
+ "Icon": "uav",
+ "Name": "MAVLink",
+ "Name[ca]": "MAVLink",
+ "Name[cs]": "MAVLink",
+ "Name[es]": "MAVLink",
+ "Name[it]": "MAVLink",
+ "Name[nl]": "MAVLink",
+ "Name[pl]": "MAVLink",
+ "Name[pt]": "MAVLink",
+ "Name[sk]": "MAVLink",
+ "Name[sv]": "MAVLink",
+ "Name[uk]": "MAVLink",
+ "Name[x-test]": "xxMAVLinkxx",
+ "ServiceTypes": [
+ "Kirogi/VehicleSupport"
+ ]
+ }
+}
diff --git a/src/plugins/mavlink/mavlinkconnection.cpp b/src/plugins/mavlink/mavlinkconnection.cpp
new file mode 100644
index 0000000..e8675ab
--- /dev/null
+++ b/src/plugins/mavlink/mavlinkconnection.cpp
@@ -0,0 +1,105 @@
+/*
+ * Copyright 2019 Patrick José Pereira
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) version 3, or any
+ * later version accepted by the membership of KDE e.V. (or its
+ * successor approved by the membership of KDE e.V.), which shall
+ * act as a proxy defined in Section 6 of version 3 of the license.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library. If not, see .
+ */
+
+#include "mavlinkconnection.h"
+
+#include
+#include
+
+MAVLinkConnection::MAVLinkConnection(const QString &vehicleName, QObject *parent)
+ : QObject(parent)
+ , m_vehicleName(vehicleName)
+ , m_connectionHost({QHostAddress("0.0.0.0"), 14550})
+{
+ qRegisterMetaType("mavlink_message_t");
+
+ // Send periodically heartbeats to say that GCS is alive
+ connect(&heartbeatTimer, &QTimer::timeout, this, [this] { sendByteArray(_heartbeatMessage); });
+ heartbeatTimer.start(1000);
+}
+
+void MAVLinkConnection::sendByteArray(const QByteArray &byteArray) const
+{
+ if (m_controlSocket) {
+ for (const auto &sender : m_senders) {
+ m_controlSocket->writeDatagram(byteArray, sender.address, sender.port);
+ }
+ }
+}
+
+void MAVLinkConnection::sendMessage(const mavlink_message_t &message) const
+{
+ const int length = mavlink_msg_to_send_buffer(m_buffer, &message);
+ sendByteArray({reinterpret_cast(m_buffer), length});
+}
+
+void MAVLinkConnection::handshake()
+{
+ initSockets();
+}
+
+void MAVLinkConnection::reset()
+{
+ if (m_controlSocket) {
+ m_controlSocket->abort();
+ delete m_controlSocket;
+ }
+
+ emit stateChanged(Kirogi::AbstractVehicle::Disconnected);
+}
+
+void MAVLinkConnection::receiveData()
+{
+ static mavlink_message_t message;
+ mavlink_status_t status;
+ while (m_controlSocket->hasPendingDatagrams()) {
+ const QNetworkDatagram &datagram = m_controlSocket->receiveDatagram();
+ m_senders.append({datagram.senderAddress(), datagram.senderPort()});
+
+ if (datagram.isValid()) {
+ const QByteArray &data = datagram.data();
+ for (const auto &byte : data) {
+ if (mavlink_parse_char(0, byte, &message, &status)) {
+ emit mavlinkMessage(message);
+ }
+ }
+ }
+ }
+
+ m_senders.erase(std::unique(m_senders.begin(), m_senders.end(), [](const auto first, const auto second) { return (first.address == second.address && first.port == second.port); }), m_senders.end());
+}
+
+void MAVLinkConnection::initSockets()
+{
+ m_controlSocket = new QUdpSocket(this);
+ QObject::connect(m_controlSocket, &QUdpSocket::readyRead, this, &MAVLinkConnection::receiveData);
+
+ m_controlSocket->setProxy(QNetworkProxy::NoProxy);
+ const bool bindState = m_controlSocket->bind(QHostAddress::AnyIPv4, m_connectionHost.port, QAbstractSocket::ReuseAddressHint | QUdpSocket::ShareAddress);
+
+ if (bindState) {
+ // The All Hosts multicast group addresses all hosts on the same network
+ // segment.
+ m_controlSocket->joinMulticastGroup(QHostAddress("224.0.0.1"));
+ }
+
+ emit stateChanged(Kirogi::AbstractVehicle::Connecting);
+ emit stateChanged(Kirogi::AbstractVehicle::Connected);
+}
diff --git a/src/plugins/mavlink/mavlinkconnection.h b/src/plugins/mavlink/mavlinkconnection.h
new file mode 100644
index 0000000..99b0f10
--- /dev/null
+++ b/src/plugins/mavlink/mavlinkconnection.h
@@ -0,0 +1,101 @@
+/*
+ * Copyright 2019 Patrick José Pereira
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) version 3, or any
+ * later version accepted by the membership of KDE e.V. (or its
+ * successor approved by the membership of KDE e.V.), which shall
+ * act as a proxy defined in Section 6 of version 3 of the license.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library. If not, see .
+ */
+
+#pragma once
+
+#include
+
+#include "abstractvehicle.h"
+
+#include
+#include
+#include
+#include
+
+// It's necessary to disable some warnings to avoid the amount of noise
+// messages from MAVLink source code
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
+#pragma GCC diagnostic ignored "-Wpedantic"
+#include "mavlink.h"
+#pragma GCC diagnostic pop
+
+namespace
+{
+/**
+ * @brief Create a QByteArray with Heartbeat Message content
+ *
+ * @return QByteArray
+ */
+QByteArray createHeartbeatMessage()
+{
+ mavlink_message_t message;
+ uint8_t buffer[1024];
+ mavlink_msg_heartbeat_pack(255, MAV_COMP_ID_MISSIONPLANNER, &message, MAV_TYPE_GCS, MAV_AUTOPILOT_INVALID, MAV_MODE_FLAG_SAFETY_ARMED, 0, MAV_STATE_ACTIVE);
+ auto length = mavlink_msg_to_send_buffer(buffer, &message);
+ return QByteArray(reinterpret_cast(buffer), length);
+}
+}
+
+class MAVLinkConnection : public QObject
+{
+ Q_OBJECT
+
+public:
+ explicit MAVLinkConnection(const QString &vehicleName, QObject *parent = nullptr);
+ ~MAVLinkConnection() = default;
+
+public Q_SLOTS:
+ void handshake();
+ void reset();
+
+ void sendByteArray(const QByteArray &byteArray) const;
+ void sendMessage(const mavlink_message_t &message) const;
+
+Q_SIGNALS:
+ void mavlinkMessage(const mavlink_message_t &message) const;
+ void stateChanged(Kirogi::AbstractVehicle::ConnectionState state) const;
+ void responseReceived(const QString &response) const;
+ void stateReceived(const QByteArray &state) const;
+
+private Q_SLOTS:
+ void receiveData();
+
+private:
+ void initSockets();
+
+ QString m_vehicleName;
+
+ struct AddressPort {
+ QHostAddress address;
+ int port;
+ };
+
+ AddressPort m_connectionHost;
+ QPointer m_controlSocket;
+ QVector m_senders;
+
+ QTimer heartbeatTimer;
+
+ // Used by sendMessage
+ mutable uint8_t m_buffer[1024];
+
+ const QByteArray _heartbeatMessage = createHeartbeatMessage();
+};
diff --git a/src/plugins/mavlink/mavlinkplugin.cpp b/src/plugins/mavlink/mavlinkplugin.cpp
new file mode 100644
index 0000000..e545a77
--- /dev/null
+++ b/src/plugins/mavlink/mavlinkplugin.cpp
@@ -0,0 +1,52 @@
+/*
+ * Copyright 2019 Patrick José Pereira
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) version 3, or any
+ * later version accepted by the membership of KDE e.V. (or its
+ * successor approved by the membership of KDE e.V.), which shall
+ * act as a proxy defined in Section 6 of version 3 of the license.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library. If not, see .
+ */
+
+#include "mavlinkplugin.h"
+#include "debug.h"
+#include "mavlinkvehicle.h"
+
+#include
+#include
+
+MAVLinkPlugin::MAVLinkPlugin(QObject *parent, const QVariantList & /* args */)
+ : Kirogi::VehicleSupportPlugin(parent)
+ , m_vehicle(nullptr)
+{
+ qCDebug(KIROGI_VEHICLESUPPORT_MAVLINK) << "MAVLink Vehicle Support Plugin initializing ...";
+
+ m_vehicle = new MAVLinkVehicle(this);
+ m_vehicle->connectToVehicle();
+
+ QMetaObject::invokeMethod(this, "vehicleAdded", Qt::QueuedConnection, Q_ARG(Kirogi::AbstractVehicle *, m_vehicle));
+}
+
+MAVLinkPlugin::~MAVLinkPlugin()
+{
+ qCDebug(KIROGI_VEHICLESUPPORT_MAVLINK) << "MAVLink Vehicle Support Plugin unloaded.";
+}
+
+QList MAVLinkPlugin::vehicles() const
+{
+ return QList() << m_vehicle;
+}
+
+K_PLUGIN_CLASS_WITH_JSON(MAVLinkPlugin, "kirogimavlinkplugin.json")
+
+#include "mavlinkplugin.moc"
diff --git a/src/plugins/mavlink/mavlinkplugin.h b/src/plugins/mavlink/mavlinkplugin.h
new file mode 100644
index 0000000..f7391a6
--- /dev/null
+++ b/src/plugins/mavlink/mavlinkplugin.h
@@ -0,0 +1,40 @@
+/*
+ * Copyright 2019 Patrick José Pereira
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) version 3, or any
+ * later version accepted by the membership of KDE e.V. (or its
+ * successor approved by the membership of KDE e.V.), which shall
+ * act as a proxy defined in Section 6 of version 3 of the license.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library. If not, see .
+ */
+
+#pragma once
+
+#include "abstractvehicle.h"
+#include "vehiclesupportplugin.h"
+
+class MAVLinkVehicle;
+
+class MAVLinkPlugin : public Kirogi::VehicleSupportPlugin
+{
+ Q_OBJECT
+
+public:
+ MAVLinkPlugin(QObject *parent, const QVariantList &args);
+ ~MAVLinkPlugin() override;
+
+ QList vehicles() const override;
+
+private:
+ MAVLinkVehicle *m_vehicle;
+};
diff --git a/src/plugins/mavlink/mavlinkvehicle.cpp b/src/plugins/mavlink/mavlinkvehicle.cpp
new file mode 100644
index 0000000..67db347
--- /dev/null
+++ b/src/plugins/mavlink/mavlinkvehicle.cpp
@@ -0,0 +1,405 @@
+/*
+ * Copyright 2019 Patrick José Pereira
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) version 3, or any
+ * later version accepted by the membership of KDE e.V. (or its
+ * successor approved by the membership of KDE e.V.), which shall
+ * act as a proxy defined in Section 6 of version 3 of the license.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library. If not, see .
+ */
+
+#include "mavlinkvehicle.h"
+#include "debug.h"
+
+#include
+#include
+#include
+
+MAVLinkVehicle::MAVLinkVehicle(QObject *parent)
+ : Kirogi::AbstractVehicle(parent)
+ , m_roll(0.0)
+ , m_pitch(0.0)
+ , m_yaw(0.0)
+ , m_signalStrength(-1)
+ , m_batteryLevel(-1)
+ , m_gpsFix(false)
+ , m_altitude(0.0)
+ , m_connection(nullptr)
+{
+ m_connection = new MAVLinkConnection(name());
+
+ // Queued connections across thread boundaries.
+ QObject::connect(m_connection, &MAVLinkConnection::stateChanged, this, &MAVLinkVehicle::setConnectionState, Qt::QueuedConnection);
+
+ QObject::connect(m_connection, &MAVLinkConnection::mavlinkMessage, this, &MAVLinkVehicle::processMavlinkMessage);
+
+ // Do the networking on a seperate thread, so our fixed-tick work never gets
+ // blocked by activity on the main thread.
+ m_connection->moveToThread(&m_connectionThread);
+ QObject::connect(&m_connectionThread, &QThread::finished, m_connection, &QObject::deleteLater);
+
+ m_connectionThread.setObjectName(QLatin1String("MAVLinkConnectionThread"));
+ m_connectionThread.start();
+}
+
+MAVLinkVehicle::~MAVLinkVehicle()
+{
+ m_connectionThread.quit();
+ m_connectionThread.wait();
+}
+
+void MAVLinkVehicle::processMavlinkMessage(const mavlink_message_t &message)
+{
+ if (message.sysid == 255) {
+ // GSC sysid
+ return;
+ }
+
+ switch (message.msgid) {
+ case MAVLINK_MSG_ID_HEARTBEAT: {
+ mavlink_heartbeat_t heartbeat;
+ mavlink_msg_heartbeat_decode(&message, &heartbeat);
+
+ switch (heartbeat.system_status) {
+ case MAV_STATE::MAV_STATE_UNINIT:
+ case MAV_STATE::MAV_STATE_BOOT:
+ case MAV_STATE::MAV_STATE_CALIBRATING:
+ case MAV_STATE::MAV_STATE_CRITICAL:
+ case MAV_STATE::MAV_STATE_EMERGENCY:
+ setConnectionState(Kirogi::AbstractVehicle::ConnectionState::Connected);
+ break;
+
+ case MAV_STATE::MAV_STATE_STANDBY:
+ case MAV_STATE::MAV_STATE_ACTIVE:
+ setConnectionState(Kirogi::AbstractVehicle::ConnectionState::Ready);
+ break;
+
+ case MAV_STATE::MAV_STATE_FLIGHT_TERMINATION:
+ case MAV_STATE::MAV_STATE_POWEROFF:
+ setConnectionState(Kirogi::AbstractVehicle::ConnectionState::Disconnected);
+ break;
+
+ // We did receive a message so it's connected..
+ default:
+ setConnectionState(Kirogi::AbstractVehicle::ConnectionState::Connected);
+ break;
+ }
+
+ break;
+ }
+
+ case MAVLINK_MSG_ID_COMMAND_ACK: {
+ mavlink_command_ack_t command_ack;
+ mavlink_msg_command_ack_decode(&message, &command_ack);
+ // TODO: Do something related to nack
+ break;
+ }
+
+ case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: {
+ mavlink_rc_channels_scaled_t rc_channels_scaled;
+ mavlink_msg_rc_channels_scaled_decode(&message, &rc_channels_scaled);
+
+ m_signalStrength = rc_channels_scaled.rssi >= 255 ? -1 : static_cast(rc_channels_scaled.rssi) / 2.54f;
+
+ emit signalStrengthChanged();
+ break;
+ }
+
+ case MAVLINK_MSG_ID_RC_CHANNELS_RAW: {
+ mavlink_rc_channels_raw_t rc_channels_raw;
+ mavlink_msg_rc_channels_raw_decode(&message, &rc_channels_raw);
+
+ m_signalStrength = rc_channels_raw.rssi >= 255 ? -1 : static_cast(rc_channels_raw.rssi) / 2.54f;
+
+ emit signalStrengthChanged();
+ break;
+ }
+
+ case MAVLINK_MSG_ID_RC_CHANNELS: {
+ mavlink_rc_channels_t rc_channels;
+ mavlink_msg_rc_channels_decode(&message, &rc_channels);
+ m_signalStrength = rc_channels.rssi >= 255 ? -1 : static_cast(rc_channels.rssi) / 2.54f;
+
+ emit signalStrengthChanged();
+ break;
+ }
+
+ case MAVLINK_MSG_ID_ALTITUDE: {
+ mavlink_altitude_t altitude;
+ mavlink_msg_altitude_decode(&message, &altitude);
+
+ m_altitude = altitude.altitude_terrain;
+
+ emit altitudeChanged();
+ break;
+ }
+
+ case MAVLINK_MSG_ID_ATTITUDE: {
+ mavlink_attitude_t attitude;
+ mavlink_msg_attitude_decode(&message, &attitude);
+
+ m_yaw = attitude.yaw;
+ m_roll = attitude.roll;
+ m_pitch = attitude.pitch;
+
+ emit rollChanged();
+ emit pitchChanged();
+ emit yawChanged();
+ emit attitudeChanged();
+ break;
+ }
+
+ case MAVLINK_MSG_ID_BATTERY_STATUS: {
+ mavlink_battery_status_t battery_status;
+ mavlink_msg_battery_status_decode(&message, &battery_status);
+
+ m_batteryLevel = battery_status.battery_remaining;
+
+ emit batteryLevelChanged();
+ break;
+ }
+
+ case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+ mavlink_global_position_int_t global_position_int;
+ mavlink_msg_global_position_int_decode(&message, &global_position_int);
+
+ // TODO: Add timer to invalidate it
+ m_gpsFix = true;
+ emit gpsFixChanged();
+
+ m_gpsPosition = QGeoCoordinate {
+ global_position_int.lat / 1e7, // degE7
+ global_position_int.lon / 1e7, // degE7
+ global_position_int.alt / 1e3 // mm
+ };
+
+ emit gpsPositionChanged();
+ break;
+ }
+
+ default: {
+ break;
+ }
+ }
+}
+
+QString MAVLinkVehicle::name() const
+{
+ return QStringLiteral("MAVLink");
+}
+
+QString MAVLinkVehicle::iconName() const
+{
+ return QStringLiteral("uav-quadcopter");
+}
+
+Kirogi::AbstractVehicle::VehicleType MAVLinkVehicle::vehicleType() const
+{
+ // TODO: Get this from mavlink messages
+ return Kirogi::AbstractVehicle::QuadCopter;
+}
+
+QList MAVLinkVehicle::supportedActions() const
+{
+ // TODO: Improve this to work with mavlink infraestructure
+ QList actions;
+ actions << Kirogi::AbstractVehicle::TakeOff;
+ actions << Kirogi::AbstractVehicle::Land;
+ actions << Kirogi::AbstractVehicle::FlipForward;
+ actions << Kirogi::AbstractVehicle::FlipBackward;
+ actions << Kirogi::AbstractVehicle::FlipLeft;
+ actions << Kirogi::AbstractVehicle::FlipRight;
+ actions << Kirogi::AbstractVehicle::SwitchPerformanceMode;
+
+ return actions;
+}
+
+void MAVLinkVehicle::requestAction(Kirogi::AbstractVehicle::VehicleAction action)
+{
+ if (!ready()) {
+ qWarning() << name()
+ << "Requested an action while not ready, aborting. Current "
+ "connection state:"
+ << connectionState();
+
+ return;
+ }
+
+ switch (action) {
+ case TakeOff: {
+ if (flying()) {
+ return;
+ }
+
+ // Set the vehicle in stabilize mode and after that arm
+ mavlink_message_t message;
+ mavlink_command_long_t command_long;
+ command_long.target_system = 1; // TODO: get from system heartbeat
+ command_long.target_component = 1; // TODO: get from system heartbeat
+ command_long.command = MAV_CMD_DO_SET_MODE;
+ command_long.confirmation = 0;
+ command_long.param1 = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+ command_long.param2 = 0; // 0 for stabilize
+ command_long.param3 = 0;
+ command_long.param4 = 0;
+ command_long.param5 = 0;
+ command_long.param6 = 0;
+ command_long.param7 = 0;
+ mavlink_msg_command_long_encode(255, MAV_COMP_ID_MISSIONPLANNER, &message, &command_long);
+ m_connection->sendMessage(message);
+
+ command_long.target_system = 1; // TODO: get from system heartbeat
+ command_long.target_component = 1; // TODO: get from system heartbeat
+ command_long.command = MAV_CMD_COMPONENT_ARM_DISARM;
+ command_long.confirmation = 0;
+ command_long.param1 = 1;
+ command_long.param2 = 0;
+ command_long.param3 = 0;
+ command_long.param4 = 0;
+ command_long.param5 = 0;
+ command_long.param6 = 0;
+ command_long.param7 = 0;
+ mavlink_msg_command_long_encode(255, MAV_COMP_ID_MISSIONPLANNER, &message, &command_long);
+ m_connection->sendMessage(message);
+
+ setFlyingState(TakingOff); // FIXME: We don't /really/ know that without
+ // looking at the response.
+ break;
+ }
+ case Land: {
+ if (!flying()) {
+ return;
+ }
+
+ // Disarm vehicle
+ mavlink_message_t message;
+ mavlink_command_long_t command_long;
+ command_long.target_system = 1; // TODO: get from system heartbeat
+ command_long.target_component = 1; // TODO: get from system heartbeat
+ command_long.command = MAV_CMD_COMPONENT_ARM_DISARM;
+ command_long.confirmation = 0;
+ command_long.param1 = 0;
+ command_long.param2 = 0;
+ command_long.param3 = 0;
+ command_long.param4 = 0;
+ command_long.param5 = 0;
+ command_long.param6 = 0;
+ command_long.param7 = 0;
+ mavlink_msg_command_long_encode(255, MAV_COMP_ID_MISSIONPLANNER, &message, &command_long);
+ m_connection->sendMessage(message);
+
+ setFlyingState(Landed); // FIXME: We don't /really/ know that without
+ // looking at the response.
+ break;
+ }
+ default: {
+ }
+ }
+}
+
+void MAVLinkVehicle::pilot(qint8 roll, qint8 pitch, qint8 yaw, qint8 gaz)
+{
+ mavlink_message_t message;
+ mavlink_manual_control_t manual_control;
+ manual_control.target = 1;
+ manual_control.x = pitch * 10; // [-1000,1000] range
+ manual_control.y = roll * 10; // [-1000,1000] range
+ manual_control.z = gaz * 10; // [-1000,1000] range
+ manual_control.r = yaw * 10; // [-1000,1000] range
+ manual_control.buttons = 0;
+ mavlink_msg_manual_control_encode(255, MAV_COMP_ID_MISSIONPLANNER, &message, &manual_control);
+ m_connection->sendMessage(message);
+}
+
+float MAVLinkVehicle::roll() const
+{
+ return m_roll;
+}
+
+float MAVLinkVehicle::pitch() const
+{
+ return m_pitch;
+}
+
+float MAVLinkVehicle::yaw() const
+{
+ return m_yaw;
+}
+
+int MAVLinkVehicle::signalStrength() const
+{
+ return m_signalStrength;
+}
+
+int MAVLinkVehicle::batteryLevel() const
+{
+ return m_batteryLevel;
+}
+
+QGeoCoordinate MAVLinkVehicle::gpsPosition() const
+{
+ return m_gpsPosition;
+}
+
+bool MAVLinkVehicle::gpsFix() const
+{
+ return m_gpsFix;
+}
+
+bool MAVLinkVehicle::gpsSupported() const
+{
+ return true;
+}
+
+float MAVLinkVehicle::altitude() const
+{
+ return m_altitude;
+}
+
+void MAVLinkVehicle::connectToVehicle()
+{
+ if (connectionState() > Disconnected) {
+ if (connectionState() > Connecting) {
+ qCDebug(KIROGI_VEHICLESUPPORT_MAVLINK) << name() << "Asked to connect when not disconnected.";
+ }
+
+ QMetaObject::invokeMethod(m_connection, "reset", Qt::BlockingQueuedConnection);
+ }
+
+ QMetaObject::invokeMethod(m_connection, "handshake", Qt::QueuedConnection);
+
+ // Keep re-trying every 3 seconds until we're successfully connected.
+ QTimer::singleShot(3000, this, [this]() {
+ if (connectionState() != Ready) {
+ qCWarning(KIROGI_VEHICLESUPPORT_MAVLINK) << name()
+ << "Unable to establish connection within 3 seconds. Starting "
+ "over.";
+
+ connectToVehicle();
+ }
+ });
+}
+
+QString MAVLinkVehicle::videoSource() const
+{
+ return QLatin1String(
+ "rtspsrc location=rtsp://192.168.99.1/media/stream2 latency=5 ! "
+ "rtph264depay ! "
+ "video/x-h264 ! "
+ "queue ! "
+ "h264parse ! "
+ "decodebin ! "
+ "glupload ! "
+ "glcolorconvert ! "
+ "qmlglsink name=sink");
+}
diff --git a/src/plugins/mavlink/mavlinkvehicle.h b/src/plugins/mavlink/mavlinkvehicle.h
new file mode 100644
index 0000000..8e0413f
--- /dev/null
+++ b/src/plugins/mavlink/mavlinkvehicle.h
@@ -0,0 +1,83 @@
+/*
+ * Copyright 2019 Patrick José Pereira
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) version 3, or any
+ * later version accepted by the membership of KDE e.V. (or its
+ * successor approved by the membership of KDE e.V.), which shall
+ * act as a proxy defined in Section 6 of version 3 of the license.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library. If not, see .
+ */
+
+#pragma once
+
+#include "abstractvehicle.h"
+#include "mavlinkconnection.h"
+
+#include
+
+class MAVLinkConnection;
+class QTimer;
+
+class MAVLinkVehicle : public Kirogi::AbstractVehicle
+{
+ Q_OBJECT
+
+public:
+ explicit MAVLinkVehicle(QObject *parent = nullptr);
+ ~MAVLinkVehicle() override;
+
+ QString name() const override;
+ QString iconName() const override;
+
+ Kirogi::AbstractVehicle::VehicleType vehicleType() const override;
+
+ QList supportedActions() const override;
+
+ Q_INVOKABLE void requestAction(Kirogi::AbstractVehicle::VehicleAction action) override;
+ Q_INVOKABLE void pilot(qint8 roll, qint8 pitch, qint8 yaw, qint8 gaz) override;
+
+ float roll() const override;
+ float pitch() const override;
+ float yaw() const override;
+
+ int signalStrength() const override;
+ int batteryLevel() const override;
+
+ bool gpsSupported() const override;
+ bool gpsFix() const override;
+ QGeoCoordinate gpsPosition() const override;
+ float altitude() const override;
+
+ QString videoSource() const override;
+
+public Q_SLOTS:
+ void connectToVehicle();
+
+private Q_SLOTS:
+ void processMavlinkMessage(const mavlink_message_t &message);
+
+private:
+ float m_roll;
+ float m_pitch;
+ float m_yaw;
+
+ int m_signalStrength;
+ int m_batteryLevel;
+
+ bool m_gpsFix;
+ QGeoCoordinate m_gpsPosition;
+ float m_altitude;
+
+ QThread m_connectionThread;
+ MAVLinkConnection *m_connection;
+};