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