diff --git a/src/gstreamer/gstreamerintegration.cpp b/src/gstreamer/gstreamerintegration.cpp index 8165d89..e4e94f5 100644 --- a/src/gstreamer/gstreamerintegration.cpp +++ b/src/gstreamer/gstreamerintegration.cpp @@ -1,153 +1,130 @@ /* * Copyright 2019 Eike Hein * * This program is free software; you can redistribute it and/or * modify it under the terms of the GNU General Public License as * published by the Free Software Foundation; either version 2 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 14 of version 3 of the license. * * This program 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 General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include "gstreamerintegration.h" +#include "videosurface.h" #include #include #include +#include +#include -class SetPlaying : public QRunnable -{ -public: - SetPlaying(GstElement *); - ~SetPlaying(); - - void run() override; - -private: - GstElement *m_pipeline; -}; - -SetPlaying::SetPlaying(GstElement *pipeline) -{ - m_pipeline = pipeline ? static_cast(gst_object_ref(pipeline)) : NULL; -} +#include +#include +#include -SetPlaying::~SetPlaying() -{ - if (m_pipeline) { - gst_object_unref(m_pipeline); - } -} -void SetPlaying::run() -{ - if (m_pipeline) { - gst_element_set_state(m_pipeline, GST_STATE_PLAYING); - } -} +Q_LOGGING_CATEGORY(videoLogging, "kirogi.videosuppoert.gstreamerintegration") GStreamerIntegration::GStreamerIntegration(QObject *parent) : QObject(parent) , m_playing(false) , m_gstPipeline(nullptr) + , m_videoSink(nullptr) , m_fallback(true) - , m_window(nullptr) { updateGstPipeline(); } GStreamerIntegration::~GStreamerIntegration() { if (m_gstPipeline) { gst_element_set_state(m_gstPipeline, GST_STATE_NULL); - gst_object_unref(m_gstPipeline); } } bool GStreamerIntegration::playing() const { return m_playing; } void GStreamerIntegration::setPlaying(bool playing) { if (m_playing != playing) { m_playing = playing; if (!m_gstPipeline) { updateGstPipeline(); } else if (m_gstPipeline->current_state == GST_STATE_PLAYING && !playing) { gst_element_set_state(m_gstPipeline, GST_STATE_PAUSED); } else if (m_gstPipeline->current_state != GST_STATE_PLAYING && playing) { updateGstPipeline(); } - emit playingChanged(); + Q_EMIT playingChanged(); } } -QString GStreamerIntegration::pipeline() const +QString GStreamerIntegration::stringPipeline() const { - return m_pipeline; + return m_stringPipeline; } -void GStreamerIntegration::setPipeline(const QString &pipeline) +void GStreamerIntegration::setStringPipeline(const QString &pipeline) { - if (m_pipeline != pipeline) { - m_pipeline = pipeline; + if (m_stringPipeline != pipeline) { + m_stringPipeline = pipeline; updateGstPipeline(); - emit pipelineChanged(); + Q_EMIT stringPipelineChanged(); } } -void GStreamerIntegration::setWindow(QQuickWindow *window) -{ - m_window = window; - updateGstPipeline(); -} - void GStreamerIntegration::updateGstPipeline() { - QString pipeline = m_pipeline; + QString pipeline = m_stringPipeline; if (pipeline.isEmpty()) { - pipeline = QLatin1String("videotestsrc pattern=snow ! video/x-raw,width=800,height=450 ! glupload ! qmlglsink name=sink"); + pipeline = QLatin1String("videotestsrc pattern=snow ! video/x-raw,width=800,height=450 !"); } + pipeline += QStringLiteral("glupload ! glcolorconvert ! qmlglsink name=sink"); if (m_gstPipeline) { gst_element_set_state(m_gstPipeline, GST_STATE_NULL); gst_object_unref(m_gstPipeline); } // If we fail to build the pipeline, we also fail to load the QML gst plugin // and the entire application will crash after that. GError *error = nullptr; m_gstPipeline = gst_parse_launch(pipeline.toLatin1().data(), &error); Q_ASSERT_X(m_gstPipeline, "gstreamer pipeline", QStringLiteral("%0 with pieline: %1").arg(error->message).arg(pipeline).toStdString().c_str()); - GstElement *sink = gst_bin_get_by_name(GST_BIN(m_gstPipeline), "sink"); + m_videoSink = gst_bin_get_by_name(GST_BIN(m_gstPipeline), "sink"); + Q_ASSERT_X(m_videoSink, "gstreamer video sink", "Could not retrieve the video sink."); - if (m_window) { - auto videoOutput = m_window->findChild("videoOutput"); - if (videoOutput) { - g_object_set(sink, "widget", videoOutput, NULL); +} - if (m_playing) { - m_window->scheduleRenderJob(new SetPlaying(m_gstPipeline), QQuickWindow::BeforeSynchronizingStage); - } - } - } +void GStreamerIntegration::init() +{ + // GStreamer needs the sink to be created before any Qml elements + // so that the Qml elements are registered in the system. so we create + // and free it. + GstElement *sink = gst_element_factory_make ("qmlglsink", nullptr); + Q_ASSERT_X(sink, "Video Initialization", "Could not find the Qml Gl Sink GStreamer Plugin, please check your installation."); gst_object_unref(sink); + + qmlRegisterType ("org.kde.kirogi", 1, 0, "VideoReceiver"); + qmlRegisterType ("org.kde.kirogi", 1, 0, "VideoSurface"); } + diff --git a/src/gstreamer/gstreamerintegration.h b/src/gstreamer/gstreamerintegration.h index 374855a..320fd40 100644 --- a/src/gstreamer/gstreamerintegration.h +++ b/src/gstreamer/gstreamerintegration.h @@ -1,63 +1,67 @@ /* * Copyright 2019 Eike Hein * * This program is free software; you can redistribute it and/or * modify it under the terms of the GNU General Public License as * published by the Free Software Foundation; either version 2 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 14 of version 3 of the license. * * This program 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 General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include +#include #include class QQuickWindow; class GStreamerIntegration : public QObject { Q_OBJECT Q_PROPERTY(bool playing READ playing WRITE setPlaying NOTIFY playingChanged) - Q_PROPERTY(QString pipeline READ pipeline WRITE setPipeline NOTIFY pipelineChanged) + Q_PROPERTY(QString stringPipeline READ stringPipeline WRITE setStringPipeline NOTIFY stringPipelineChanged) public: explicit GStreamerIntegration(QObject *parent = nullptr); ~GStreamerIntegration(); bool playing() const; void setPlaying(bool playing); - QString pipeline() const; - void setPipeline(const QString &pipeline); + QString stringPipeline() const; + void setStringPipeline(const QString &pipeline); + static void init(); - void setWindow(QQuickWindow *window); + GstElement *videoSink() const { return m_videoSink; } + GstElement *pipeline() const { return m_gstPipeline; } Q_SIGNALS: void playingChanged() const; - void pipelineChanged() const; + void stringPipelineChanged() const; private: void updateGstPipeline(); bool m_playing; - QString m_pipeline; + QString m_stringPipeline; GstElement *m_gstPipeline; + GstElement *m_videoSink; bool m_fallback; - - QQuickWindow *m_window; }; + +Q_DECLARE_LOGGING_CATEGORY(videoLogging) diff --git a/src/main.cpp b/src/main.cpp index 3334c76..0dbbbc1 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,155 +1,142 @@ /* * Copyright 2019 Eike Hein * * This program is free software; you can redistribute it and/or * modify it under the terms of the GNU General Public License as * published by the Free Software Foundation; either version 2 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 14 of version 3 of the license. * * This program 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 General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include "gstreamer/gstreamerintegration.h" #include "permissions.h" #include "settings.h" #include #ifndef Q_OS_ANDROID #include #endif #include #include #ifdef Q_OS_ANDROID #include #include #else #include #endif #include #include #include #include #include #include #ifdef Q_OS_ANDROID extern "C" gboolean gst_qt_android_init(GError **error); Q_DECL_EXPORT #endif int main(int argc, char *argv[]) { int ret; #ifndef Q_OS_ANDROID gst_init(&argc, &argv); + GStreamerIntegration::init(); #else if (!gst_qt_android_init(NULL)) { return -1; } #endif QGuiApplication::setAttribute(Qt::AA_EnableHighDpiScaling); #ifdef Q_OS_ANDROID QGuiApplication app(argc, argv); #else QApplication app(argc, argv); #endif KLocalizedString::setApplicationDomain("kirogi"); KAboutData aboutData(QStringLiteral("kirogi"), xi18nc("@title", "Kirogi"), QStringLiteral("0.1-dev"), xi18nc("@title", "A ground control application for drones."), KAboutLicense::GPL, xi18nc("@info:credit", "(c) 2019 The Kirogi Team"), QString(), QStringLiteral("https://www.kirogi.org/")); aboutData.setOrganizationDomain(QByteArray("kde.org")); aboutData.setProductName(QByteArray("kirogi")); aboutData.addAuthor(xi18nc("@info:credit", "Eike Hein"), xi18nc("@info:credit", "Founder, Lead Developer"), QStringLiteral("hein@kde.org")); aboutData.addAuthor(xi18nc("@info:credit", "Patrick José Pereira"), xi18nc("@info:credit", "Developer"), QStringLiteral("patrickjp@kde.org")); aboutData.addAuthor(xi18nc("@info:credit", "Rafael Brandmaier"), xi18nc("@info:credit", "Application icon"), QStringLiteral("rafael.brandmaier@kdemail.net")); aboutData.addAuthor(xi18nc("@info:credit", "L. 'AsmoArael' C."), xi18nc("@info:credit", "Mascot artwork"), QStringLiteral("lc.jarryh99@outlook.fr")); KAboutData::setApplicationData(aboutData); QCommandLineParser parser; parser.addHelpOption(); parser.addVersionOption(); aboutData.setupCommandLine(&parser); parser.process(app); aboutData.processCommandLine(&parser); app.setApplicationName(aboutData.componentName()); app.setApplicationDisplayName(aboutData.displayName()); app.setOrganizationDomain(aboutData.organizationDomain()); app.setApplicationVersion(aboutData.version()); app.setWindowIcon(QIcon::fromTheme(QStringLiteral("kirogi"))); #ifndef Q_OS_ANDROID KCrash::initialize(); #endif QQmlApplicationEngine engine(&app); /* Add relative path to import paths to cover the case when the application * is not installed in the system. */ engine.addImportPath(QCoreApplication::applicationDirPath() + "/../lib/qml"); // For i18n. engine.rootContext()->setContextObject(new KLocalizedContext(&engine)); engine.rootContext()->setContextProperty(QStringLiteral("kirogiAboutData"), QVariant::fromValue(aboutData)); engine.rootContext()->setContextProperty(QStringLiteral("kirogiSettings"), Settings::self()); - // Initialize video stack. - GStreamerIntegration *videoPlayer = new GStreamerIntegration(&app); - engine.rootContext()->setContextProperty(QStringLiteral("videoPlayer"), QVariant::fromValue(videoPlayer)); - engine.rootContext()->setContextProperty(QStringLiteral("locationPermissions"), QVariant::fromValue(new Permissions({QStringLiteral("android.permission.ACCESS_COARSE_LOCATION"), QStringLiteral("android.permission.ACCESS_FINE_LOCATION")}, &app))); engine.load(QUrl(QStringLiteral("qrc:///main.qml"))); if (engine.rootObjects().isEmpty()) { qCritical() << "Application failed to load."; - - gst_deinit(); - return -1; } - videoPlayer->setWindow(qobject_cast(engine.rootObjects().first())); - #ifdef Q_OS_ANDROID QtAndroid::hideSplashScreen(); #endif ret = app.exec(); - - delete videoPlayer; - - gst_deinit(); - return ret; } diff --git a/src/plugins/mavlink/mavlinkvehicle.cpp b/src/plugins/mavlink/mavlinkvehicle.cpp index 577f4b3..1f1f272 100644 --- a/src/plugins/mavlink/mavlinkvehicle.cpp +++ b/src/plugins/mavlink/mavlinkvehicle.cpp @@ -1,443 +1,440 @@ /* * 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 "parametermodel.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_connection(nullptr) { m_connection = new MAVLinkConnection(name()); // Queued connections across thread boundaries. QObject::connect(m_connection, &MAVLinkConnection::stateChanged, this, &MAVLinkVehicle::setConnectionState, Qt::QueuedConnection); // Request vehicle parameters if we are in connected state. QObject::connect(this, &MAVLinkVehicle::connectionStateChanged, this, &MAVLinkVehicle::fetchParameters, Qt::QueuedConnection); QObject::connect(m_connection, &MAVLinkConnection::mavlinkMessage, this, &MAVLinkVehicle::processMavlinkMessage); // Do the networking on a separate 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) { 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_altitudeSource.altitudeMessage = true; m_altitudeSource.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: { // Both latitude and longitude are provided in degE7 // Altitude is provided in mm mavlink_global_position_int_t global_position_int; mavlink_msg_global_position_int_decode(&message, &global_position_int); m_gpsFix = !(qFuzzyCompare(global_position_int.lat, 0.0) && qFuzzyCompare(global_position_int.lon, 0.0)); emit gpsFixChanged(); if (m_gpsFix) { m_gpsPosition.setLatitude(global_position_int.lat / 1e7); m_gpsPosition.setLongitude(global_position_int.lon / 1e7); } else { // Invalidate position if there is no fix m_gpsPosition.setLatitude(qQNaN()); m_gpsPosition.setLongitude(qQNaN()); } m_gpsPosition.setAltitude(global_position_int.alt / 1e3); emit gpsPositionChanged(); // Check if altitude message was received if (!m_altitudeSource.altitudeMessage) { m_altitudeSource.altitude = m_gpsPosition.altitude(); emit altitudeChanged(); } break; } case MAVLINK_MSG_ID_PARAM_VALUE: { mavlink_param_value_t param_value; mavlink_msg_param_value_decode(&message, ¶m_value); parameters()->update({ {ParameterModel::Roles::Name, QString(param_value.param_id)}, {ParameterModel::Roles::Value, static_cast(param_value.param_value)}, {ParameterModel::Roles::Index, static_cast(param_value.param_index)}, }); } 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_altitudeSource.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(); } }); } void MAVLinkVehicle::fetchParameters() const { // We need to handle this in a different way to ensure that we are going to receive all parameters. static bool requested = false; if (connectionState() < Kirogi::AbstractVehicle::ConnectionState::Connected || requested) { return; } requested = true; mavlink_message_t message; mavlink_param_request_list_t param_request_list; param_request_list.target_system = 1; param_request_list.target_component = 1; mavlink_msg_param_request_list_encode(255, MAV_COMP_ID_MISSIONPLANNER, &message, ¶m_request_list); m_connection->sendMessage(message); } 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"); + "decodebin ! "); } diff --git a/src/plugins/parrot/parrotvehicle.cpp b/src/plugins/parrot/parrotvehicle.cpp index a319be0..3ef0690 100644 --- a/src/plugins/parrot/parrotvehicle.cpp +++ b/src/plugins/parrot/parrotvehicle.cpp @@ -1,1175 +1,1175 @@ /* * Copyright 2019 Eike Hein * * 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 "parrotvehicle.h" #include "debug.h" #include "parrotconnection.h" #include "parrotprotocol.h" #include #include #include #include ParrotVehicle::ParrotVehicle(Type type, const QString &hostName, int port, const QString &productSerial, QObject *parent) : Kirogi::AbstractVehicle(parent) , m_type(type) , m_hostName(hostName) , m_productSerial(productSerial) , m_initialized(0) , m_piloting(false) , m_maxRollPitchSpeed(0.0) , m_maxRollPitchSpeedMin(0.0) , m_maxRollPitchSpeedMax(0.0) , m_maxYawSpeed(0.0) , m_maxYawSpeedMin(0.0) , m_maxYawSpeedMax(0.0) , m_maxGazSpeed(0.0) , m_maxGazSpeedMin(0.0) , m_maxGazSpeedMax(0.0) , m_maxTilt(0.0) , m_maxTiltMin(0.0) , m_maxTiltMax(0.0) , m_bankedTurns(false) , m_geofence(false) , m_maxAltitude(0.0) , m_maxAltitudeMin(0.0) , m_maxAltitudeMax(0.0) , m_maxDistance(0.0) , m_maxDistanceMin(0.0) , m_maxDistanceMax(0.0) , m_roll(0.0) , m_pitch(0.0) , m_yaw(0.0) , m_defaultCameraOrientationTilt(0) , m_defaultCameraOrientationPan(0) , m_signalStrength(100) , m_batteryLevel(100) , m_gpsFix(false) , m_altitude(0.0) , m_speed(0.0) , m_videoStreamEnabled(false) , m_videoStreamMode(0) , m_videoStabilizationMode(0) , m_canTakePicture(false) , m_isRecordingVideo(false) , m_numberOfFlights(0) , m_lastFlightDuration(0) , m_connection(nullptr) { m_connection = new ParrotConnection(type, name(), hostName, port); // Queued connections across thread boundaries. QObject::connect(m_connection, &ParrotConnection::stateChanged, this, &ParrotVehicle::setConnectionState, Qt::QueuedConnection); QObject::connect( m_connection, &ParrotConnection::stateChanged, this, [this](AbstractVehicle::ConnectionState state) { if (state == Connected) { initVehicle(); } if (state == Disconnected) { // FIXME Reset more stuff. m_initialized = 0; setFlyingState(Unknown); m_videoStreamEnabled = false; emit videoStreamEnabledChanged(); emit videoSourceChanged(); m_canTakePicture = false; emit canTakePictureChanged(); m_isRecordingVideo = false; emit isRecordingVideoChanged(); m_gpsFix = false; emit gpsFixChanged(); } }, Qt::QueuedConnection); QObject::connect(m_connection, &ParrotConnection::commandReceived, this, &ParrotVehicle::processIncomingCommand, Qt::QueuedConnection); // 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("ParrotConnectionThread")); m_connectionThread.start(); } ParrotVehicle::~ParrotVehicle() { m_connectionThread.quit(); m_connectionThread.wait(); } QString ParrotVehicle::name() const { QString prettyName; if (m_type == Bebop2) { prettyName = i18n("Bebop 2"); } else if (m_type == Anafi) { prettyName = i18n("Anafi"); } else { prettyName = i18n("Unknown Parrot Model"); } return i18nc("%1 = Model name, %2 = Hostname", "%1 (%2)", prettyName, (m_productSerial.isEmpty() ? m_hostName : m_productSerial)); } QString ParrotVehicle::iconName() const { if (m_type == Bebop2) { return QStringLiteral("uav-quadcopter"); ; } return Kirogi::AbstractVehicle::iconName(); } Kirogi::AbstractVehicle::VehicleType ParrotVehicle::vehicleType() const { if (m_type == Bebop2) { return Kirogi::AbstractVehicle::QuadCopter; } return Kirogi::AbstractVehicle::UnknownVehicleType; } QList ParrotVehicle::supportedActions() const { auto actions = QList({Kirogi::AbstractVehicle::TakeOff, Kirogi::AbstractVehicle::Land, Kirogi::AbstractVehicle::FlatTrim, Kirogi::AbstractVehicle::SwitchPerformanceMode, Kirogi::AbstractVehicle::SetMaxRollSpeed, Kirogi::AbstractVehicle::SetMaxPitchSpeed, Kirogi::AbstractVehicle::SetMaxYawSpeed, Kirogi::AbstractVehicle::SetMaxTilt, Kirogi::AbstractVehicle::SetMaxGazSpeed, Kirogi::AbstractVehicle::ToggleBankedTurns, Kirogi::AbstractVehicle::ToggleGeofence, Kirogi::AbstractVehicle::SetMaxAltitude, Kirogi::AbstractVehicle::SetMaxDistance, Kirogi::AbstractVehicle::ToggleVideoStream, Kirogi::AbstractVehicle::RecordVideo}); // FIXME TODO: Implement camera mode changing. // actions << Kirogi::AbstractVehicle::TakePicture; // FIXME TODO: Look for alternatives for the Anafi. if (m_type == Bebop2) { actions += QList( {Kirogi::AbstractVehicle::FlipForward, Kirogi::AbstractVehicle::FlipBackward, Kirogi::AbstractVehicle::FlipLeft, Kirogi::AbstractVehicle::FlipRight, Kirogi::AbstractVehicle::ToggleVideoStabilization}); } return actions; } bool ParrotVehicle::piloting() const { return m_piloting; } void ParrotVehicle::requestMoveTo(QGeoCoordinate destination) { if (!flying()) { qCWarning(KIROGI_VEHICLESUPPORT_PARROT) << name() << "Move-to request while not flying, aborting. Current flying state:" << flyingState(); return; } if (!gpsFix()) { qCWarning(KIROGI_VEHICLESUPPORT_PARROT) << name() << "Move-to request without a GPS fix, aborting."; return; } sendCommand(Parrot::Ardrone3PilotingmoveTo, {destination.latitude(), destination.longitude(), destination.altitude(), 1, 0.0}); } void ParrotVehicle::setPiloting(bool piloting) { if (m_piloting != piloting) { if (piloting) { sendCommand(Parrot::CommonControllerisPiloting, {1}); } else { sendCommand(Parrot::CommonControllerisPiloting, {0}); } } } void ParrotVehicle::requestAction(Kirogi::AbstractVehicle::VehicleAction action) { if (!ready()) { qCWarning(KIROGI_VEHICLESUPPORT_PARROT) << name() << "Requested an action while not ready, aborting. Current connection state:" << connectionState(); return; } switch (action) { case TakeOff: { if (flying()) { return; } sendCommand(Parrot::Ardrone3PilotingTakeOff); break; } case Land: { if (!flying()) { return; } sendCommand(Parrot::Ardrone3PilotingLanding); break; } case FlatTrim: { sendCommand(Parrot::Ardrone3PilotingFlatTrim); break; } case FlipForward: { if (!flying()) { return; } sendCommand(Parrot::Ardrone3AnimationsFlip, {0}); break; } case FlipBackward: { if (!flying()) { return; } sendCommand(Parrot::Ardrone3AnimationsFlip, {1}); break; } case FlipLeft: { if (!flying()) { return; } sendCommand(Parrot::Ardrone3AnimationsFlip, {3}); break; } case FlipRight: { if (!flying()) { return; } sendCommand(Parrot::Ardrone3AnimationsFlip, {2}); break; } case ToggleBankedTurns: { requestEnableBankedTurns(!bankedTurns()); break; } case ToggleGeofence: { requestEnableGeofence(!geofence()); break; } case ToggleVideoStabilization: { requestEnableVideoStabilization(!videoStabilization()); break; } case TakePicture: { sendCommand(Parrot::Ardrone3MediaRecordPictureV2); break; } case RecordVideo: { if (m_isRecordingVideo) { sendCommand(Parrot::Ardrone3MediaRecordVideoV2, {0}); } else { sendCommand(Parrot::Ardrone3MediaRecordVideoV2, {1}); } break; } default: { AbstractVehicle::requestAction(action); } } } void ParrotVehicle::pilot(qint8 roll, qint8 pitch, qint8 yaw, qint8 gaz) { if (!flying()) { qCWarning(KIROGI_VEHICLESUPPORT_PARROT) << name() << "Piloting request while not flying, aborting. Current flying state:" << flyingState(); return; } QMetaObject::invokeMethod(m_connection, "pilot", Qt::QueuedConnection, Q_ARG(qint8, roll), Q_ARG(qint8, pitch), Q_ARG(qint8, yaw), Q_ARG(qint8, gaz)); } Kirogi::AbstractVehicle::PerformanceMode ParrotVehicle::performanceMode() const { // FIXME TODO: These values are coming from FreeFlight, but particularly // the SportPerformance values don't feel satisfactory to me. if (ready()) { if (m_type == Bebop2) { if (m_maxRollPitchSpeed == 80 && m_maxYawSpeed == 13 && m_maxGazSpeed == 1 && m_maxTilt == 8 && m_bankedTurns == true && m_videoStreamMode == 1 && videoStabilization()) { return FilmPerformance; } else if (m_maxRollPitchSpeed == 200 && m_maxYawSpeed == 150 && m_maxGazSpeed == 5 && m_maxTilt == 35 && m_bankedTurns == false && m_videoStreamMode == 0 && !videoStabilization()) { return SportPerformance; } } else if (m_type == Anafi) { // FIXME TODO: Look for an equivalent to `m_videoStreamMode` for the Anafi. // FIXME TODO: Look for an equivalent to `videoStabilization()` for the // Anafi. // FIXME TODO: FreeFlight 6 also has a 'Max camera tilt speed' setting we // may need before long. if (m_maxRollPitchSpeed == 80 && m_maxYawSpeed == 10 && m_maxGazSpeed == 1 && m_maxTilt == 10 && m_bankedTurns == true) { return FilmPerformance; } else if (m_maxRollPitchSpeed == 200 && m_maxYawSpeed == 30 && m_maxGazSpeed == 2 // FreeFlight 6 uses 1.9, but there's some weird precision mismatch issue. && m_maxTilt == 25 && m_bankedTurns == false) { return SportPerformance; } } } return CustomPerformance; } void ParrotVehicle::requestPerformanceMode(Kirogi::AbstractVehicle::PerformanceMode mode) { if (performanceMode() != mode) { if (m_type == Bebop2) { switch (mode) { case FilmPerformance: { sendCommand(Parrot::Ardrone3SpeedSettingsMaxPitchRollRotationSpeed, {80}); sendCommand(Parrot::Ardrone3SpeedSettingsMaxRotationSpeed, {13}); sendCommand(Parrot::Ardrone3SpeedSettingsMaxVerticalSpeed, {1}); sendCommand(Parrot::Ardrone3PilotingSettingsMaxTilt, {8}); sendCommand(Parrot::Ardrone3PilotingSettingsBankedTurn, {1}); sendCommand(Parrot::Ardrone3MediaStreamingVideoStreamMode, {1}); requestEnableVideoStabilization(true); break; } case SportPerformance: { sendCommand(Parrot::Ardrone3SpeedSettingsMaxPitchRollRotationSpeed, {200}); sendCommand(Parrot::Ardrone3SpeedSettingsMaxRotationSpeed, {150}); sendCommand(Parrot::Ardrone3SpeedSettingsMaxVerticalSpeed, {5}); sendCommand(Parrot::Ardrone3PilotingSettingsMaxTilt, {35}); sendCommand(Parrot::Ardrone3PilotingSettingsBankedTurn, {0}); sendCommand(Parrot::Ardrone3MediaStreamingVideoStreamMode, {0}); requestEnableVideoStabilization(false); break; } default: { } } } else if (m_type == Anafi) { switch (mode) { case FilmPerformance: { sendCommand(Parrot::Ardrone3SpeedSettingsMaxPitchRollRotationSpeed, {80}); sendCommand(Parrot::Ardrone3SpeedSettingsMaxRotationSpeed, {10}); sendCommand(Parrot::Ardrone3SpeedSettingsMaxVerticalSpeed, {1}); sendCommand(Parrot::Ardrone3PilotingSettingsMaxTilt, {10}); sendCommand(Parrot::Ardrone3PilotingSettingsBankedTurn, {1}); break; } case SportPerformance: { sendCommand(Parrot::Ardrone3SpeedSettingsMaxPitchRollRotationSpeed, {200}); sendCommand(Parrot::Ardrone3SpeedSettingsMaxRotationSpeed, {30}); sendCommand(Parrot::Ardrone3SpeedSettingsMaxVerticalSpeed, {2}); sendCommand(Parrot::Ardrone3PilotingSettingsMaxTilt, {25}); sendCommand(Parrot::Ardrone3PilotingSettingsBankedTurn, {0}); break; } default: { } } } } } float ParrotVehicle::maxRollSpeed() const { return m_maxRollPitchSpeed; } void ParrotVehicle::requestMaxRollSpeed(float speed) { if (m_maxRollPitchSpeed != speed) { sendCommand(Parrot::Ardrone3SpeedSettingsMaxPitchRollRotationSpeed, {speed}); } } float ParrotVehicle::maxRollSpeedMin() const { return m_maxRollPitchSpeedMin; } float ParrotVehicle::maxRollSpeedMax() const { return m_maxRollPitchSpeedMax; } float ParrotVehicle::maxPitchSpeed() const { return m_maxRollPitchSpeed; } void ParrotVehicle::requestMaxPitchSpeed(float speed) { if (m_maxRollPitchSpeed != speed) { sendCommand(Parrot::Ardrone3SpeedSettingsMaxPitchRollRotationSpeed, {speed}); } } float ParrotVehicle::maxPitchSpeedMin() const { return m_maxRollPitchSpeedMin; } float ParrotVehicle::maxPitchSpeedMax() const { return m_maxRollPitchSpeedMax; } float ParrotVehicle::maxYawSpeed() const { return m_maxYawSpeed; } void ParrotVehicle::requestMaxYawSpeed(float speed) { if (m_maxYawSpeed != speed) { sendCommand(Parrot::Ardrone3SpeedSettingsMaxRotationSpeed, {speed}); } } float ParrotVehicle::maxYawSpeedMin() const { return m_maxYawSpeedMin; } float ParrotVehicle::maxYawSpeedMax() const { return m_maxYawSpeedMax; } float ParrotVehicle::maxGazSpeed() const { return m_maxGazSpeed; } void ParrotVehicle::requestMaxGazSpeed(float speed) { if (m_maxGazSpeed != speed) { sendCommand(Parrot::Ardrone3SpeedSettingsMaxVerticalSpeed, {speed}); } } float ParrotVehicle::maxGazSpeedMin() const { return m_maxGazSpeedMin; } float ParrotVehicle::maxGazSpeedMax() const { return m_maxGazSpeedMax; } float ParrotVehicle::maxTilt() const { return m_maxTilt; } void ParrotVehicle::requestMaxTilt(float tilt) { if (m_maxTilt != tilt) { sendCommand(Parrot::Ardrone3PilotingSettingsMaxTilt, {tilt}); } } float ParrotVehicle::maxTiltMin() const { return m_maxTiltMin; } float ParrotVehicle::maxTiltMax() const { return m_maxTiltMax; } bool ParrotVehicle::bankedTurns() const { return m_bankedTurns; } void ParrotVehicle::requestEnableBankedTurns(bool enable) { if (m_bankedTurns != enable) { if (enable) { sendCommand(Parrot::Ardrone3PilotingSettingsBankedTurn, {1}); } else { sendCommand(Parrot::Ardrone3PilotingSettingsBankedTurn, {0}); } } } void ParrotVehicle::requestEnableGeofence(bool enable) { if (m_geofence != enable) { if (enable) { sendCommand(Parrot::Ardrone3PilotingSettingsNoFlyOverMaxDistance, {1}); } else { sendCommand(Parrot::Ardrone3PilotingSettingsNoFlyOverMaxDistance, {0}); } } } float ParrotVehicle::maxAltitude() const { return m_maxAltitude; } void ParrotVehicle::requestMaxAltitude(float altitude) { if (m_maxAltitude != altitude) { sendCommand(Parrot::Ardrone3PilotingSettingsMaxAltitude, {altitude}); } } float ParrotVehicle::maxAltitudeMin() const { return m_maxAltitudeMin; } float ParrotVehicle::maxAltitudeMax() const { return m_maxAltitudeMax; } bool ParrotVehicle::geofence() const { return m_geofence; } float ParrotVehicle::maxDistance() const { return m_maxDistance; } void ParrotVehicle::requestMaxDistance(float distance) { if (m_maxDistance != distance) { sendCommand(Parrot::Ardrone3PilotingSettingsMaxDistance, {distance}); } } float ParrotVehicle::maxDistanceMin() const { return m_maxDistanceMin; } float ParrotVehicle::maxDistanceMax() const { return m_maxDistanceMax; } float ParrotVehicle::roll() const { return m_roll; } float ParrotVehicle::pitch() const { return m_pitch; } float ParrotVehicle::yaw() const { return m_yaw; } int ParrotVehicle::signalStrength() const { if (m_signalStrength <= -100) { return 0; } else if (m_signalStrength >= -50) { return 100; } return 2 * (m_signalStrength + 100); } int ParrotVehicle::batteryLevel() const { return m_batteryLevel; } bool ParrotVehicle::gpsSupported() const { return true; } bool ParrotVehicle::gpsFix() const { return m_gpsFix; } QGeoCoordinate ParrotVehicle::gpsPosition() const { return m_gpsPosition; } float ParrotVehicle::altitude() const { return m_altitude; } void ParrotVehicle::setControllerGpsPosition(const QGeoCoordinate &position) { sendCommand(Parrot::Ardrone3GPSSettingsSendControllerGPS, {position.latitude(), position.longitude(), position.altitude()}); } void ParrotVehicle::requestReturnHome() { sendCommand(Parrot::Ardrone3PilotingNavigateHome); } float ParrotVehicle::speed() const { return m_speed; } QString ParrotVehicle::videoSource() const { if (videoStreamEnabled()) { if (m_type == Bebop2) { - return QLatin1String("udpsrc port=55004 ! application/x-rtp, clock-rate=90000,payload=96 ! rtph264depay ! video/x-h264 ! queue ! h264parse ! decodebin ! glupload ! glcolorconvert ! qmlglsink name=sink"); + return QLatin1String("udpsrc port=55004 ! application/x-rtp, clock-rate=90000,payload=96 ! rtph264depay ! video/x-h264 ! queue ! h264parse ! decodebin !"); } else if (m_type == Anafi) { - return QLatin1String("rtspsrc location=rtsp://192.168.42.1/live latency=5 ! rtph264depay ! video/x-h264 ! queue ! h264parse ! decodebin ! glupload ! glcolorconvert ! qmlglsink name=sink"); + return QLatin1String("rtspsrc location=rtsp://192.168.42.1/live latency=5 ! rtph264depay ! video/x-h264 ! queue ! h264parse ! decodebin !"); } } return QString(); } bool ParrotVehicle::videoStreamEnabled() const { return m_videoStreamEnabled; } void ParrotVehicle::requestEnableVideoStream(bool enable) { if (m_videoStreamEnabled != enable) { if (enable) { sendCommand(Parrot::Ardrone3MediaStreamingVideoEnable, {1}); sendCommand(Parrot::Ardrone3PictureSettingsVideoAutorecordSelection, {0, 0}); // The Anafi won't send us Ardrone3MediaStreamingStateVideoEnableChanged. if (m_type == Anafi) { m_videoStreamEnabled = true; emit videoStreamEnabledChanged(); emit videoSourceChanged(); } } else { // The Anafi won't send us Ardrone3MediaStreamingStateVideoEnableChanged. if (m_type == Anafi) { m_videoStreamEnabled = false; emit videoStreamEnabledChanged(); emit videoSourceChanged(); } sendCommand(Parrot::Ardrone3MediaStreamingVideoEnable, {0}); } } } bool ParrotVehicle::videoStabilization() const { return (m_videoStabilizationMode == 0); } void ParrotVehicle::requestEnableVideoStabilization(bool enable) { const int requestedMode = enable ? 0 : 3; if (m_videoStabilizationMode != requestedMode) { if (enable) { sendCommand(Parrot::Ardrone3PictureSettingsVideoStabilizationMode, {0}); } else { sendCommand(Parrot::Ardrone3PictureSettingsVideoStabilizationMode, {3}); } } } bool ParrotVehicle::canTakePicture() const { return m_canTakePicture; } bool ParrotVehicle::isRecordingVideo() const { return m_isRecordingVideo; } quint16 ParrotVehicle::numberOfFlights() const { return m_numberOfFlights; } quint16 ParrotVehicle::lastFlightDuration() const { return m_lastFlightDuration; } void ParrotVehicle::connectToVehicle() { if (connectionState() > Disconnected) { qCDebug(KIROGI_VEHICLESUPPORT_PARROT) << name() << "Asked to connect when not disconnected."; QMetaObject::invokeMethod(m_connection, "reset", Qt::BlockingQueuedConnection); } QMetaObject::invokeMethod(m_connection, "handshake", Qt::QueuedConnection, Q_ARG(QString, m_productSerial)); // Keep re-trying every 3 seconds until we're successfully connected. QTimer::singleShot(3000, this, [this]() { if (connectionState() != Ready) { qCWarning(KIROGI_VEHICLESUPPORT_PARROT) << name() << "Unable to establish connection within 3 seconds. Starting over."; connectToVehicle(); } }); } void ParrotVehicle::processIncomingCommand(const ParrotCommand &command) { const Parrot::Command id = command.id(); switch (id) { case Parrot::Ardrone3PilotingStateAttitudeChanged: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); s.setFloatingPointPrecision(QDataStream::SinglePrecision); s >> m_roll >> m_pitch >> m_yaw; emit attitudeChanged(); emit rollChanged(); emit pitchChanged(); emit yawChanged(); break; } case Parrot::Ardrone3PilotingStateAltitudeChanged: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); s.setFloatingPointPrecision(QDataStream::DoublePrecision); s >> m_altitude; emit altitudeChanged(); break; } case Parrot::Ardrone3PilotingStateSpeedChanged: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); s.setFloatingPointPrecision(QDataStream::SinglePrecision); float speedX; float speedY; float speedZ; s >> speedX >> speedY >> speedZ; m_speed = std::max({speedX, speedY, speedZ}); emit speedChanged(); break; } case Parrot::Ardrone3PilotingStatePositionChanged: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); s.setFloatingPointPrecision(QDataStream::DoublePrecision); qreal latitude; qreal longitude; qreal altitude; s >> latitude >> longitude >> altitude; QGeoCoordinate newPos(latitude, longitude, altitude); if (newPos.isValid()) { m_gpsPosition = newPos; } emit gpsPositionChanged(); break; } case Parrot::CommonCommonStateWifiSignalChanged: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); s >> m_signalStrength; emit signalStrengthChanged(); break; } case Parrot::Ardrone3GPSSettingsStateGPSFixStateChanged: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); quint8 gpsFix = 0; s >> gpsFix; m_gpsFix = (gpsFix == 1); emit gpsFixChanged(); break; } case Parrot::CommonCommonStateBatteryStateChanged: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); qint8 battery; s >> battery; m_batteryLevel = battery; emit batteryLevelChanged(); break; } case Parrot::Ardrone3PilotingStateFlyingStateChanged: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); qint32 flyingState; s >> flyingState; switch (flyingState) { case 0: { setFlyingState(Landed); break; } case 1: { setFlyingState(TakingOff); break; } case 2: { setFlyingState(Hovering); break; } case 3: { setFlyingState(Flying); break; } case 4: { setFlyingState(Landing); break; } default: { setFlyingState(Unknown); } } break; } case Parrot::Ardrone3SpeedSettingsStateMaxPitchRollRotationSpeedChanged: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); s.setFloatingPointPrecision(QDataStream::SinglePrecision); s >> m_maxRollPitchSpeed >> m_maxRollPitchSpeedMin >> m_maxRollPitchSpeedMax; emit maxRollSpeedChanged(); emit maxRollSpeedMinChanged(); emit maxRollSpeedMaxChanged(); emit maxPitchSpeedChanged(); emit maxPitchSpeedMinChanged(); emit maxPitchSpeedMaxChanged(); emit performanceModeChanged(); break; } case Parrot::Ardrone3SpeedSettingsStateMaxRotationSpeedChanged: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); s.setFloatingPointPrecision(QDataStream::SinglePrecision); s >> m_maxYawSpeed >> m_maxYawSpeedMin >> m_maxYawSpeedMax; emit maxYawSpeedChanged(); emit maxYawSpeedMinChanged(); emit maxYawSpeedMaxChanged(); emit performanceModeChanged(); break; } case Parrot::Ardrone3SpeedSettingsStateMaxVerticalSpeedChanged: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); s.setFloatingPointPrecision(QDataStream::SinglePrecision); s >> m_maxGazSpeed >> m_maxGazSpeedMin >> m_maxGazSpeedMax; emit maxGazSpeedChanged(); emit maxGazSpeedMinChanged(); emit maxGazSpeedMaxChanged(); emit performanceModeChanged(); break; } case Parrot::Ardrone3PilotingSettingsStateMaxTiltChanged: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); s.setFloatingPointPrecision(QDataStream::SinglePrecision); s >> m_maxTilt >> m_maxTiltMin >> m_maxTiltMax; emit maxTiltChanged(); emit maxTiltMinChanged(); emit maxTiltMaxChanged(); emit performanceModeChanged(); break; } case Parrot::Ardrone3PilotingSettingsStateBankedTurnChanged: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); quint8 bankedTurns; s >> bankedTurns; m_bankedTurns = (bankedTurns == 1); emit bankedTurnsChanged(); emit performanceModeChanged(); break; } case Parrot::Ardrone3PilotingSettingsStateMaxAltitudeChanged: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); s.setFloatingPointPrecision(QDataStream::SinglePrecision); s >> m_maxAltitude >> m_maxAltitudeMin >> m_maxAltitudeMax; emit maxAltitudeChanged(); emit maxAltitudeMinChanged(); emit maxAltitudeMaxChanged(); break; } case Parrot::Ardrone3PilotingSettingsStateNoFlyOverMaxDistanceChanged: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); quint8 geofence; s >> geofence; m_geofence = (geofence == 1); emit geofenceChanged(); break; } case Parrot::Ardrone3PilotingSettingsStateMaxDistanceChanged: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); s.setFloatingPointPrecision(QDataStream::SinglePrecision); s >> m_maxDistance >> m_maxDistanceMin >> m_maxDistanceMax; emit maxDistanceChanged(); emit maxDistanceMinChanged(); emit maxDistanceMaxChanged(); break; } case Parrot::Ardrone3SettingsStateMotorFlightsStatusChanged: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); s >> m_numberOfFlights >> m_lastFlightDuration; emit numberOfFlightsChanged(); emit lastFlightDurationChanged(); break; } case Parrot::Ardrone3MediaStreamingStateVideoEnableChanged: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); qint32 videoStreamEnabled; s >> videoStreamEnabled; m_videoStreamEnabled = (videoStreamEnabled == 0); emit videoStreamEnabledChanged(); emit videoSourceChanged(); break; } case Parrot::Ardrone3MediaStreamingStateVideoStreamModeChanged: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); s >> m_videoStreamMode; emit performanceModeChanged(); break; } case Parrot::Ardrone3PictureSettingsStateVideoStabilizationModeChanged: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); s >> m_videoStabilizationMode; emit performanceModeChanged(); break; } case Parrot::Ardrone3MediaRecordStatePictureStateChangedV2: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); qint32 canTakePictureState; s >> canTakePictureState; m_canTakePicture = (canTakePictureState == 0); emit canTakePictureChanged(); break; } case Parrot::Ardrone3MediaRecordStateVideoStateChangedV2: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); qint32 videoRecordingState; s >> videoRecordingState; m_isRecordingVideo = (videoRecordingState == 1); emit isRecordingVideoChanged(); break; } case Parrot::Ardrone3CameraStatedefaultCameraOrientationV2: { QDataStream s(command.data); s.setByteOrder(QDataStream::LittleEndian); s >> m_defaultCameraOrientationTilt >> m_defaultCameraOrientationPan; break; } case Parrot::CommonCommonStateAllStatesChanged: case Parrot::CommonSettingsStateAllSettingsChanged: { ++m_initialized; // If we got both of those for the first time within a session, // we're now Ready. if (m_initialized == 2) { qCDebug(KIROGI_VEHICLESUPPORT_PARROT) << name() << "All initial state received."; setConnectionState(Ready); } break; } case Parrot::UnknownCommand: { // qCWarning(KIROGI_VEHICLESUPPORT_PARROT) << "Unknown command:" // << command.tuple.productId // << command.tuple.classId // << command.tuple.commandId; break; } default: { // qCWarning(KIROGI_VEHICLESUPPORT_PARROT) << "No handler for command:" << id; } } } void ParrotVehicle::initVehicle() { qCDebug(KIROGI_VEHICLESUPPORT_PARROT) << name() << "Sending initialization commands to vehicle" << name() << "..."; const QDateTime &now = QDateTime::currentDateTime(); sendCommand(Parrot::CommonCommonCurrentDate, {now.date().toString(Qt::ISODate)}, true); sendCommand(Parrot::CommonCommonCurrentTime, {now.time().toString(QLatin1String("THHmmss+0000"))}, true); sendCommand(Parrot::CommonCommonAllStates, QVariantList(), true); sendCommand(Parrot::CommonSettingsAllSettings, QVariantList(), true); // FIXME TODO: Possibly delay connectionState=Ready until the all of following // are done, too. if (m_piloting) { sendCommand(Parrot::CommonControllerisPiloting, {1}); } else { sendCommand(Parrot::CommonControllerisPiloting, {0}); } requestPerformanceMode(FilmPerformance); requestMaxAltitude(30); requestMaxDistance(50); requestEnableGeofence(true); // FIXME TODO: Try to center camera upon successful connection. // sendCommand(Parrot::Ardrone3CameraOrientationV2, {m_defaultCameraOrientationTilt, // m_defaultCameraOrientationPan}); // Set preferred home type to pilot. sendCommand(Parrot::Ardrone3GPSSettingsHomeType, {1}); } void ParrotVehicle::sendCommand(Parrot::Command command, const QVariantList &arguments, bool retryForever) { if (!connected()) { qCWarning(KIROGI_VEHICLESUPPORT_PARROT) << name() << "Request to send command" << command << "rejected. Connection not ready. Current connection state:" << connectionState(); return; } QMetaObject::invokeMethod(m_connection, "sendCommand", Qt::QueuedConnection, Q_ARG(Parrot::Command, command), Q_ARG(QVariantList, arguments), Q_ARG(bool, retryForever)); } diff --git a/src/ui/FlightControls.qml b/src/ui/FlightControls.qml index 4344ce5..851ff79 100644 --- a/src/ui/FlightControls.qml +++ b/src/ui/FlightControls.qml @@ -1,959 +1,966 @@ /* * Copyright 2019 Eike Hein * * This program is free software; you can redistribute it and/or * modify it under the terms of the GNU General Public License as * published by the Free Software Foundation; either version 2 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 14 of version 3 of the license. * * This program 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 General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ import QtQuick 2.12 import QtQuick.Controls 2.12 as QQC2 import org.kde.kirigami 2.6 as Kirigami import org.freedesktop.gstreamer.GLVideoItem 1.0 -import org.kde.kirogi 0.1 as Kirogi +import org.kde.kirogi 1.0 as Kirogi Kirigami.Page { id: page LayoutMirroring.enabled: false LayoutMirroring.childrenInherit: true readonly property int yardstick: Math.min(parent.width, parent.height) readonly property bool touched: leftTouchPoint.active || rightTouchPoint.active property alias gamepad: gamepadLoader.item leftPadding: 0 rightPadding: 0 topPadding: 0 bottomPadding: 0 Connections { target: kirogi onCurrentPageChanged: updatePilotingState() onCurrentVehicleChanged: updatePilotingState() onReadyChanged: updatePilotingState() } function updatePilotingState() { var vehicle = kirogi.currentVehicle; if (!vehicle || !vehicle.ready) { return; } vehicle.setPiloting(kirogi.currentPage == page); if (kirogi.currentPage == page && !kirogi.currentVehicle.videoStreamEnabled && vehicle.isActionSupported(Kirogi.AbstractVehicle.ToggleVideoStream)) { vehicle.requestEnableVideoStream(true); } } Image { id: cameraStream anchors.fill: parent fillMode: Image.PreserveAspectCrop smooth: true source: "fallback.jpg" Connections { target: kirogi onCurrentVehicleChanged: { - videoPlayer.playing = kirogi.currentVehicle != null; + videoSurface.playing = kirogi.currentVehicle != null; } onCurrentPageChanged: { - videoPlayer.playing = (kirogi.currentPage == page || kirogi.currentVehicle); + videoSurface.playing = (kirogi.currentPage == page || kirogi.currentVehicle); } } Binding { - target: videoPlayer + target: videoReceiver property: "pipeline" value: kirogi.currentVehicle ? kirogi.currentVehicle.videoSource : "" } + Kirogi.VideoReceiver { + id: videoReceiver + } + + Kirogi.VideoSurface { + id: videoSurface + videoItem: video + videoReceiver: videoReceiver + } + GstGLVideoItem { + id: video anchors.centerIn: parent width: parent.width height: parent.width/1.77 // FIXME: Work with non-16:9 videos. - - objectName: "videoOutput" } Rectangle { id: videoOverlay anchors.fill: parent visible: !kirogi.currentVehicle || kirogi.currentVehicle.videoSource === "" opacity: 0.4 color: "black" } } // FIXME TODO: This is a workaround around the org.kde.desktop+Breeze style engine // hijacking drag on the window. TapHandler { enabled: !Kirigami.Settings.isMobile } Item { anchors.left: parent.left width: parent.width / 2 height: parent.height PointHandler { id: leftTouchPoint enabled: inputMode.selectedMode == 0 || kirogiSettings.alwaysShowDPads grabPermissions: PointerHandler.ApprovesTakeOverByAnything | PointerHandler.ApprovesCancellation } } Item { anchors.right: parent.right width: parent.width / 2 height: parent.height PointHandler { id: rightTouchPoint enabled: inputMode.selectedMode == 0 || kirogiSettings.alwaysShowDPads } } TouchButton { id: leftButton anchors.top: leftPillBox.bottom anchors.topMargin: Math.round(leftPillBox.y * 1.5) anchors.left: parent.left anchors.leftMargin: leftPillBox.y background: cameraStream icon: kirogi.currentVehicle ? kirogi.currentVehicle.iconName : "uav" toolTipText: i18nc("%1 = Keyboard shortcut", "Drone (%1)", vehiclePageAction.__shortcut.nativeText) onTapped: switchApplicationPage(vehiclePage) } PillBox { id: launchButton anchors.horizontalCenter: parent.horizontalCenter anchors.top: parent.top anchors.topMargin: Kirigami.Units.smallSpacing width: Math.min(launchButtonLabel.implicitWidth + Kirigami.Units.smallSpacing * 4, rightPillBox.x - leftPillBox.x - leftPillBox.width - (leftPillBox.x * 2)) height: 2 * Math.round((leftPillBox.height * 1.12) / 2); readonly property var __color: { if (launchButtonMouseArea.containsMouse) { return Kirigami.Theme.hoverColor; } if (kirogi.connected) { if (kirogi.flying) { return "red"; } else if (kirogi.ready) { return "green"; } else { return "yellow"; } } return "red"; } background: cameraStream backgroundColor: "dark" + __color backgroundOpacity: 0.4 borderWidth: 2 borderRadius: height / 4 borderColor: launchButtonLabel.color Text { id: launchButtonLabel anchors.fill: parent font.pixelSize: parent.height * 0.7 font.bold: true color: launchButton.__color horizontalAlignment: Text.AlignHCenter verticalAlignment: Text.AlignVCenter elide: Text.ElideRight text: { if (kirogi.connected) { if (kirogi.flying) { return i18n("LAND"); } else if (kirogi.ready) { return i18n("TAKE OFF"); } else { return i18n("PREPARING") } } return i18n("DISCONNECTED"); } Behavior on color { enabled: !launchButtonMouseArea.pressed ColorAnimation { duration: Kirigami.Units.shortDuration } } } MouseArea { id: launchButtonMouseArea anchors.fill: parent enabled: kirogi.ready hoverEnabled: enabled onClicked: { if (kirogi.flying) { kirogi.currentVehicle.requestLand(); } else { kirogi.currentVehicle.requestTakeOff(); } } } } TouchButton { id: rightButton anchors.top: rightPillBox.bottom anchors.topMargin: leftButton.anchors.topMargin anchors.right: parent.right anchors.rightMargin: leftButton.anchors.leftMargin background: cameraStream icon: "map-flat" toolTipText: i18nc("%1 = Keyboard shortcut", "Navigation Map (%1)", navigationMapPageAction.__shortcut.nativeText) onTapped: switchApplicationPage(navigationMapPage) } ModeRocker { id: flightMode enabled: kirogi.ready anchors.left: parent.left anchors.verticalCenter: shotButton.verticalCenter background: cameraStream selectedMode: { if (kirogi.ready) { if (kirogi.currentVehicle.performanceMode == Kirogi.AbstractVehicle.FilmPerformance) { return 0; } else if (kirogi.currentVehicle.performanceMode == Kirogi.AbstractVehicle.SportPerformance) { return 1; } } return -1; } firstLabelText: i18n("FILM") firstToolTipText: i18n("Fly Slow") secondLabelText: i18n("SPORT") secondToolTipText: i18n("Fly Fast") onModeTapped: { if (selectedMode == mode) { return; } if (mode == 0) { kirogi.currentVehicle.requestPerformanceMode(Kirogi.AbstractVehicle.FilmPerformance); } else if (mode == 1) { kirogi.currentVehicle.requestPerformanceMode(Kirogi.AbstractVehicle.SportPerformance); } } Component.onCompleted: { var handleWidth = kirogi.LayoutMirroring.enabled ? parent.width - globalDrawer.handle.x : globalDrawer.handle.x + globalDrawer.handle.width; anchors.leftMargin = globalDrawer.modal ? handleWidth + leftPillBox.y : leftPillBox.y; } } ModeRocker { id: inputMode visible: gamepad && gamepad.connected && !kirogiSettings.alwaysShowDPads anchors.verticalCenter: shotButton.verticalCenter anchors.left: flightMode.right anchors.leftMargin: leftPillBox.y firstLabelText: i18n("SCREEN") firstIconSource: Kirigami.Settings.isMobile ? "phone-symbolic" : "computer-symbolic" firstToolTipText: i18n("Use Virtual D-Pads") secondLabelText: i18n("CONTROLLER") secondIconSource: "folder-games-symbolic" secondToolTipText: i18n("Use Gamepad Controller") showLabels: false showIcons: true selectedMode: kirogiSettings.lastInputMode onModeTapped: { selectedMode = mode; kirogiSettings.lastInputMode = selectedMode; kirogiSettings.save(); } } ModeRocker { id: shotMode enabled: kirogi.ready anchors.right: shotButton.right anchors.rightMargin: shotButton.width / 2 anchors.verticalCenter: shotButton.verticalCenter width: (implicitWidth + shotButton.width / 2) - Kirigami.Units.largeSpacing property int requestedMode: 0 background: cameraStream firstModeEnabled: enabled && kirogi.currentVehicle.isActionSupported(Kirogi.AbstractVehicle.RecordVideo) secondModeEnabled: enabled && kirogi.currentVehicle.isActionSupported(Kirogi.AbstractVehicle.TakePicture) firstLabelText: i18n("VIDEO") firstIconSource: "emblem-videos-symbolic" firstToolTipText: i18n("Record Videos") secondLabelText: i18n("PHOTO") secondIconSource: "emblem-photos-symbolic" secondToolTipText: i18n("Take Photos") showLabels: false showIcons: true selectedMode: kirogi.ready ? requestedMode : -1 onModeTapped: requestedMode = mode } TouchButton { id: shotButton enabled: { if (!kirogi.ready) { return false; } if ((shotMode.selectedMode == 0 && !kirogi.currentVehicle.isActionSupported(Kirogi.AbstractVehicle.RecordVideo)) || (shotMode.selectedMode == 1 && (!kirogi.currentVehicle.isActionSupported(Kirogi.AbstractVehicle.TakePicture) || !kirogi.currentVehicle.canTakePicture))) { return false; } return true; } anchors.right: parent.right anchors.rightMargin: flightMode.anchors.leftMargin anchors.bottom: parent.bottom anchors.bottomMargin: launchButton.anchors.topMargin background: cameraStream icon: "media-record-symbolic" iconColor: shotMode.selectedMode == 0 && (kirogi.currentVehicle && kirogi.currentVehicle.isRecordingVideo) ? "red" : "white" toolTipText: { if (shotMode.selectedMode) { return i18n("Take Photo"); } else if (kirogi.currentVehicle && kirogi.currentVehicle.isRecordingVideo) { return i18n("Stop Recording Video"); } return i18n("Record Video"); } onTapped: { if (!kirogi.ready) { return; } if (shotMode.selectedMode == 0) { kirogi.currentVehicle.requestAction(Kirogi.AbstractVehicle.RecordVideo); } else if (shotMode.selectedMode == 1) { kirogi.currentVehicle.requestAction(Kirogi.AbstractVehicle.TakePicture); } } } TouchDPad { id: leftDPad visible: inputMode.selectedMode == 0 || kirogiSettings.alwaysShowDPads || (gamepad && !gamepad.connected) anchors.left: parent.left anchors.leftMargin: yardstick * 0.18 anchors.bottom: parent.bottom anchors.bottomMargin: yardstick * 0.20 width: Math.min(yardstick * 0.45, parent.width / 4) height: width background: cameraStream leftIcon: "edit-undo" leftToolTipText: i18n("Turn Left") rightIcon: "edit-redo" rightToolTipText: i18n("Turn Right") topIcon: "arrow-up" topToolTipText: i18n("Move Up") bottomIcon: "arrow-down" bottomToolTipText: i18n("Move Down") onXChanged: moved = aboutToMove onAxisXChanged: { kirogi.currentVehicle.pilot(rightDPad.axisX, rightDPad.axisY, leftDPad.axisX, leftDPad.axisY); } onAxisYChanged: { kirogi.currentVehicle.pilot(rightDPad.axisX, rightDPad.axisY, leftDPad.axisX, leftDPad.axisY); } touchPos: { if (moved && leftTouchPoint) { var xDifference = 0; if (leftTouchPoint.point.scenePosition.x > leftTouchPoint.point.scenePressPosition.x) { xDifference = xDifference + Math.abs(leftTouchPoint.point.scenePressPosition.x - leftTouchPoint.point.scenePosition.x); } else { xDifference = xDifference - Math.abs(leftTouchPoint.point.scenePressPosition.x - leftTouchPoint.point.scenePosition.x) } var x = leftDPad.x + leftDPad.width / 2 + xDifference; var yDifference = 0; if (leftTouchPoint.point.scenePosition.y > leftTouchPoint.point.scenePressPosition.y) { yDifference = yDifference + Math.abs(leftTouchPoint.point.scenePressPosition.y - leftTouchPoint.point.scenePosition.y); } else { yDifference = yDifference - Math.abs(leftTouchPoint.point.scenePressPosition.y - leftTouchPoint.point.scenePosition.y) } var y = leftDPad.y + leftDPad.height / 2 + yDifference; return parent.mapToItem(leftDPad, x, y); } return null; } states: [ State { name: "inactive" AnchorChanges { target: leftDPad anchors.left: parent.left anchors.bottom: parent.bottom } PropertyChanges { target: leftDPad aboutToMove: false moved: false } }, State { name: "active" when: leftTouchPoint.active AnchorChanges { target: leftDPad anchors.left: undefined anchors.bottom: undefined } PropertyChanges { target: leftDPad aboutToMove: true x: Math.min((parent.width / 2) - width, Math.max(0, leftTouchPoint.point.scenePressPosition.x - width / 2)) y: Math.min(parent.height - height, Math.max(0, leftTouchPoint.point.scenePressPosition.y - height / 2)) } } ] } TouchDPad { id: rightDPad visible: leftDPad.visible width: leftDPad.height height: width anchors.right: parent.right anchors.rightMargin: leftDPad.anchors.leftMargin anchors.bottom: parent.bottom anchors.bottomMargin: leftDPad.anchors.bottomMargin background: cameraStream leftIcon: "go-previous" leftToolTipText: i18n("Move Left") rightIcon: "go-next" rightToolTipText: i18n("Move Right") topIcon: "go-up" topToolTipText: i18n("Move Forward") bottomIcon: "go-down" bottomToolTipText: i18n("Move Backward") onXChanged: moved = aboutToMove onAxisXChanged: { kirogi.currentVehicle.pilot(rightDPad.axisX, rightDPad.axisY, leftDPad.axisX, leftDPad.axisY); } onAxisYChanged: { kirogi.currentVehicle.pilot(rightDPad.axisX, rightDPad.axisY, leftDPad.axisX, leftDPad.axisY); } touchPos: { if (moved && rightTouchPoint.active) { var xDifference = 0; if (rightTouchPoint.point.scenePosition.x > rightTouchPoint.point.scenePressPosition.x) { xDifference = xDifference + Math.abs(rightTouchPoint.point.scenePressPosition.x - rightTouchPoint.point.scenePosition.x); } else { xDifference = xDifference - Math.abs(rightTouchPoint.point.scenePressPosition.x - rightTouchPoint.point.scenePosition.x) } var x = rightDPad.x + rightDPad.width / 2 + xDifference; var yDifference = 0; if (rightTouchPoint.point.scenePosition.y > rightTouchPoint.point.scenePressPosition.y) { yDifference = yDifference + Math.abs(rightTouchPoint.point.scenePressPosition.y - rightTouchPoint.point.scenePosition.y); } else { yDifference = yDifference - Math.abs(rightTouchPoint.point.scenePressPosition.y - rightTouchPoint.point.scenePosition.y) } var y = rightDPad.y + rightDPad.height / 2 + yDifference; return parent.mapToItem(rightDPad, x, y); } return null; } states: [ State { name: "inactive" AnchorChanges { target: rightDPad anchors.left: parent.right anchors.bottom: parent.bottom } PropertyChanges { target: rightDPad aboutToMove: false moved: false } }, State { name: "active" when: rightTouchPoint.active AnchorChanges { target: rightDPad anchors.right: undefined anchors.bottom: undefined } PropertyChanges { target: rightDPad aboutToMove: true x: Math.max(parent.width / 2, Math.min(parent.width - width, rightTouchPoint.point.scenePressPosition.x - width / 2)) y: Math.min(parent.height - height, Math.max(0, rightTouchPoint.point.scenePressPosition.y - height / 2)) } } ] } PillBox { id: leftPillBox anchors.verticalCenter: launchButton.verticalCenter anchors.left: parent.left anchors.leftMargin: y width: leftPillBoxContents.implicitWidth + Kirigami.Units.largeSpacing * 4 height: 2 * Math.round((Math.max(Kirigami.Units.iconSizes.small, fontMetrics.height) + Kirigami.Units.smallSpacing * 3) / 2); background: cameraStream Row { id: leftPillBoxContents anchors.horizontalCenter: parent.horizontalCenter height: parent.height spacing: Kirigami.Units.largeSpacing Kirigami.Icon { anchors.verticalCenter: parent.verticalCenter width: Kirigami.Units.iconSizes.small height: width color: "white" smooth: true isMask: true source: "speedometer" } QQC2.Label { anchors.verticalCenter: parent.verticalCenter width: kirogi.currentVehicle ? Math.round(Math.max(implicitWidth, fontMetrics.tightBoundingRect(i18n("%1 m/s", "0.0")).width)) : Math.round(implicitWidth) color: "white" horizontalAlignment: Text.AlignRight text: { if(kirogi.currentVehicle) { return i18n("%1 m/s", kirogi.flying ? kirogi.currentVehicle.speed : "0"); } return i18n("– m/s"); } } PillBoxSeparator {} Kirigami.Icon { anchors.verticalCenter: parent.verticalCenter width: Kirigami.Units.iconSizes.small height: width color: "white" smooth: true isMask: true source: "kruler-west" } QQC2.Label { anchors.verticalCenter: parent.verticalCenter width: kirogi.ready ? Math.round(Math.max(implicitWidth, fontMetrics.tightBoundingRect(i18n("%1 m", "0.0")).width)) : Math.round(implicitWidth) color: "white" horizontalAlignment: Text.AlignRight text: { if (kirogi.currentVehicle) { return i18n("%1 m", kirogi.currentVehicle.altitude.toFixed(2)) } return i18n("– m"); } } PillBoxSeparator {} Kirigami.Icon { anchors.verticalCenter: parent.verticalCenter width: Kirigami.Units.iconSizes.small height: width color: "white" smooth: true isMask: true source: "kruler-south" } QQC2.Label { anchors.verticalCenter: parent.verticalCenter width: kirogi.currentVehicle ? Math.round(Math.max(implicitWidth, fontMetrics.tightBoundingRect(i18n("%1 m", "0.0")).width)) : Math.round(implicitWidth) color: "white" horizontalAlignment: Text.AlignRight text: { if (kirogi.currentVehicle && kirogi.currentVehicle.distance >= 0) { return i18n("%1 m", kirogi.currentVehicle.distance.toFixed(1)); } if (kirogi.distance !== 0) { return i18n("%1 m", kirogi.distance.toFixed(1)); } return i18n("– m"); } } } } PillBox { id: rightPillBox anchors.verticalCenter: launchButton.verticalCenter anchors.right: parent.right anchors.rightMargin: leftPillBox.anchors.leftMargin width: rightPillBoxContents.implicitWidth + Kirigami.Units.largeSpacing * 4 height: leftPillBox.height background: cameraStream Row { id: rightPillBoxContents x: Kirigami.Units.largeSpacing anchors.horizontalCenter: parent.horizontalCenter height: parent.height spacing: Kirigami.Units.largeSpacing Kirigami.Icon { anchors.verticalCenter: parent.verticalCenter width: Kirigami.Units.iconSizes.small height: width color: "white" smooth: true isMask: true source: "clock" } QQC2.Label { anchors.verticalCenter: parent.verticalCenter width: kirogi.currentVehicle ? Math.round(Math.max(implicitWidth, fontMetrics.tightBoundingRect(i18n("%1 m", "0:00")).width)) : Math.round(implicitWidth) color: "white" horizontalAlignment: Text.AlignRight text: { if (kirogi.ready) { var time = kirogi.flying ? kirogi.currentVehicle.flightTime : 0; return i18n("%1 min", (time - (time %= 60)) / 60 + (9 < time ?':':':0') + time); } return i18n("– min"); } } PillBoxSeparator {} Kirigami.Icon { anchors.verticalCenter: parent.verticalCenter width: Kirigami.Units.iconSizes.small height: width color: "white" smooth: true isMask: kirogi.currentVehicle source: { if (kirogi.currentVehicle) { if (kirogi.currentVehicle.signalStrength === 0) { return "network-wireless-connected-00"; } else if (kirogi.currentVehicle.signalStrength < 25) { return "network-wireless-connected-25"; } else if (kirogi.currentVehicle.signalStrength < 50) { return "network-wireless-connected-50"; } else if (kirogi.currentVehicle.signalStrength < 75) { return "network-wireless-connected-75"; } else if (kirogi.currentVehicle.signalStrength <= 100) { return "network-wireless-connected-100"; } } if (kirogi.connected) { return "network-wireless-acquiring"; } return "network-wireless-disconnected"; } } QQC2.Label { anchors.verticalCenter: parent.verticalCenter width: kirogi.currentVehicle ? Math.round(Math.max(implicitWidth, fontMetrics.tightBoundingRect(i18n("%1%", 00)).width)) : Math.round(implicitWidth) color: "white" horizontalAlignment: Text.AlignRight text: kirogi.currentVehicle ? i18n("%1%", kirogi.currentVehicle.signalStrength) : i18n("N/A") } PillBoxSeparator {} Kirigami.Icon { anchors.verticalCenter: parent.verticalCenter width: Kirigami.Units.iconSizes.small height: width color: "white" smooth: true isMask: kirogi.currentVehicle source: { if (kirogi.currentVehicle) { var roundedBatteryLevel = Math.round(kirogi.currentVehicle.batteryLevel / 10); return "battery-" + roundedBatteryLevel.toString().padStart(2, "0") + "0"; } return "battery-missing"; } } QQC2.Label { anchors.verticalCenter: parent.verticalCenter width: kirogi.currentVehicle ? Math.round(Math.max(implicitWidth, fontMetrics.tightBoundingRect(i18n("%1%", 00)).width)) : Math.round(implicitWidth) color: "white" horizontalAlignment: Text.AlignRight text: kirogi.currentVehicle ? i18n("%1%", kirogi.currentVehicle.batteryLevel) : i18n("N/A") } } } PitchBar { id: pitchBar anchors.centerIn: parent width: yardstick * 0.04 height: parent.height * 0.6 pitch: kirogi.currentVehicle ? kirogi.currentVehicle.pitch * (180/Math.PI) : 0.0 } VirtualHorizon { id: virtualHorizon anchors.centerIn: pitchBar width: yardstick * 0.2 roll: kirogi.currentVehicle ? kirogi.currentVehicle.roll * (180/Math.PI) : 0 } YawBar { id: yawBar tickWidth: 10 anchors.left: leftDPad.horizontalCenter anchors.right: rightDPad.horizontalCenter anchors.bottom: inputMode.visible ? inputMode.top : parent.bottom yaw: kirogi.currentVehicle ? kirogi.currentVehicle.yaw * (180 / Math.PI) : 0 } VehicleActionsDrawer { enabled: kirogi.currentPage == page // FIXME TODO: Why doesn't come down from page.enabled? width: Kirigami.Units.gridUnit * 19 edge: kirogi.LayoutMirroring.enabled ? Qt.LeftEdge : Qt.RightEdge handleClosedIcon.source: "configure" } Loader { id: gamepadLoader source: "Gamepad.qml" asynchronous: true // FIXME TODO: QtGamepad currently causes performance problems on // Android (blocking multi-tasking) that need to be investigated. active: !Kirigami.Settings.isMobile } - - Component.onCompleted: videoPlayer.playing = kirogiSettings.flying }