From f91c5bef0813f0aabd4939278d0c3e1c536b1868 Mon Sep 17 00:00:00 2001 From: Gus Grubba Date: Mon, 24 Oct 2016 21:32:22 -0400 Subject: [PATCH] Setting up Mavlink logging Start/Stop (and receiving the data) --- src/Vehicle/Vehicle.cc | 63 +++++++++++++ src/Vehicle/Vehicle.h | 10 +++ src/uas/MavlinkLogManager.cc | 120 ++++++++++++++++++++++--- src/uas/MavlinkLogManager.h | 39 +++++--- src/uas/UASInterface.h | 1 + src/ui/preferences/MavlinkSettings.qml | 57 +++++++++++- 6 files changed, 264 insertions(+), 26 deletions(-) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 09480d7bd..ff154aa86 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -400,6 +400,7 @@ Vehicle::resetCounters() void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message) { + if (message.sysid != _id && message.sysid != 0) { return; } @@ -488,6 +489,12 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS: _handleHilActuatorControls(message); break; + case MAVLINK_MSG_ID_LOGGING_DATA: + _handleMavlinkLoggingData(message); + break; + case MAVLINK_MSG_ID_LOGGING_DATA_ACKED: + _handleMavlinkLoggingDataAcked(message); + break; // Following are ArduPilot dialect messages @@ -1959,6 +1966,62 @@ VehicleGPSFactGroup::VehicleGPSFactGroup(QObject* parent) _courseOverGroundFact.setRawValue(std::numeric_limits::quiet_NaN()); } +//----------------------------------------------------------------------------- +void +Vehicle::startMavlinkLog() +{ + doCommandLong(defaultComponentId(), MAV_CMD_LOGGING_START); +} + +//----------------------------------------------------------------------------- +void +Vehicle::stopMavlinkLog() +{ + doCommandLong(defaultComponentId(), MAV_CMD_LOGGING_STOP); +} + +//----------------------------------------------------------------------------- +void +Vehicle::_ackMavlinkLogData(uint16_t sequence) +{ + mavlink_message_t msg; + mavlink_logging_ack_t ack; + ack.sequence = sequence; + ack.target_component = defaultComponentId(); + ack.target_system = id(); + mavlink_msg_logging_ack_encode_chan( + _mavlink->getSystemId(), + _mavlink->getComponentId(), + priorityLink()->mavlinkChannel(), + &msg, + &ack); + sendMessageOnLink(priorityLink(), msg); +} + +//----------------------------------------------------------------------------- +void +Vehicle::_handleMavlinkLoggingData(mavlink_message_t& message) +{ + qDebug() << "MAVLINK_MSG_ID_LOGGING_DATA"; + mavlink_logging_data_t log; + mavlink_msg_logging_data_decode(&message, &log); + emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence, log.length, log.first_message_offset, log.data, false); +} + +//----------------------------------------------------------------------------- +void +Vehicle::_handleMavlinkLoggingDataAcked(mavlink_message_t& message) +{ + qDebug() << "MAVLINK_MSG_ID_LOGGING_DATA_ACKED"; + mavlink_logging_data_t log; + mavlink_msg_logging_data_decode(&message, &log); + _ackMavlinkLogData(log.sequence); + emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence, log.length, log.first_message_offset, log.data, true); +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- + void VehicleGPSFactGroup::setVehicle(Vehicle* vehicle) { _vehicle = vehicle; diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index c2b3bfbed..808d65092 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -485,6 +485,10 @@ public: int flowImageIndex() { return _flowImageIndex; } + //-- Mavlink Logging + void startMavlinkLog(); + void stopMavlinkLog(); + /// Requests the specified data stream from the vehicle /// @param stream Stream which is being requested /// @param rate Rate at which to send stream in Hz @@ -638,6 +642,9 @@ signals: void mavlinkScaledImu2(mavlink_message_t message); void mavlinkScaledImu3(mavlink_message_t message); + // Mavlink Log Download + void mavlinkLogData (Vehicle* vehicle, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t length, uint8_t first_message, const uint8_t* data, bool acked); + private slots: void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message); void _linkInactiveOrDeleted(LinkInterface* link); @@ -695,6 +702,9 @@ private: void _connectionActive(void); void _say(const QString& text); QString _vehicleIdSpeech(void); + void _handleMavlinkLoggingData(mavlink_message_t& message); + void _handleMavlinkLoggingDataAcked(mavlink_message_t& message); + void _ackMavlinkLogData(uint16_t sequence); private: int _id; ///< Mavlink system id diff --git a/src/uas/MavlinkLogManager.cc b/src/uas/MavlinkLogManager.cc index 2b45f8a23..9563dd1a4 100644 --- a/src/uas/MavlinkLogManager.cc +++ b/src/uas/MavlinkLogManager.cc @@ -21,13 +21,13 @@ QGC_LOGGING_CATEGORY(MavlinkLogManagerLog, "MavlinkLogManagerLog") -static const char* kEmailAddressKey = "MavlinkLogEmail"; -static const char* kDescriptionsKey = "MavlinkLogDescription"; -static const char* kDefaultDescr = "QGroundControl Session"; -static const char* kPx4URLKey = "MavlinkLogURL"; -static const char* kDefaultPx4URL = "http://logs.px4.io/upload"; -static const char* kEnableAutologKey= "EnableAutologKey"; - +static const char* kEmailAddressKey = "MavlinkLogEmail"; +static const char* kDescriptionsKey = "MavlinkLogDescription"; +static const char* kDefaultDescr = "QGroundControl Session"; +static const char* kPx4URLKey = "MavlinkLogURL"; +static const char* kDefaultPx4URL = "http://logs.px4.io/upload"; +static const char* kEnableAutoUploadKey = "EnableAutoUploadKey"; +static const char* kEnableAutoStartKey = "EnableAutoStartKey"; //----------------------------------------------------------------------------- MavlinkLogFiles::MavlinkLogFiles(MavlinkLogManager *manager, const QString& filePath) @@ -70,16 +70,20 @@ MavlinkLogFiles::setProgress(qreal progress) //----------------------------------------------------------------------------- MavlinkLogManager::MavlinkLogManager(QGCApplication* app) : QGCTool(app) - , _enableAutolog(true) + , _enableAutoUpload(true) + , _enableAutoStart(true) , _nam(NULL) , _currentLogfile(NULL) + , _vehicle(NULL) + , _logRunning(false) { //-- Get saved settings QSettings settings; setEmailAddress(settings.value(kEmailAddressKey, QString()).toString()); setDescription(settings.value(kDescriptionsKey, QString(kDefaultDescr)).toString()); setUploadURL(settings.value(kPx4URLKey, QString(kDefaultPx4URL)).toString()); - setEnableAutolog(settings.value(kEnableAutologKey, true).toBool()); + setEnableAutoUpload(settings.value(kEnableAutoUploadKey, true).toBool()); + setEnableAutoStart(settings.value(kEnableAutoStartKey, true).toBool()); //-- Logging location _logPath = QStandardPaths::writableLocation(QStandardPaths::AppDataLocation); _logPath += "/MavlinkLogs"; @@ -109,6 +113,7 @@ MavlinkLogManager::setToolbox(QGCToolbox *toolbox) QGCTool::setToolbox(toolbox); QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); qmlRegisterUncreatableType("QGroundControl.MavlinkLogManager", 1, 0, "MavlinkLogManager", "Reference only"); + connect(toolbox->multiVehicleManager(), &MultiVehicleManager::activeVehicleChanged, this, &MavlinkLogManager::_activeVehicleChanged); // _uploadURL = "http://192.168.1.21/px4"; // _uploadURL = "http://192.168.1.9:8080"; @@ -153,12 +158,22 @@ MavlinkLogManager::setUploadURL(QString url) //----------------------------------------------------------------------------- void -MavlinkLogManager::setEnableAutolog(bool enable) +MavlinkLogManager::setEnableAutoUpload(bool enable) +{ + _enableAutoUpload = enable; + QSettings settings; + settings.setValue(kEnableAutoUploadKey, enable); + emit enableAutoUploadChanged(); +} + +//----------------------------------------------------------------------------- +void +MavlinkLogManager::setEnableAutoStart(bool enable) { - _enableAutolog = enable; + _enableAutoStart = enable; QSettings settings; - settings.setValue(kEnableAutologKey, enable); - emit enableAutologChanged(); + settings.setValue(kEnableAutoStartKey, enable); + emit enableAutoStartChanged(); } //----------------------------------------------------------------------------- @@ -218,6 +233,28 @@ MavlinkLogManager::cancelUpload() } } +//----------------------------------------------------------------------------- +void +MavlinkLogManager::startLogging() +{ + if(_vehicle) { + _vehicle->startMavlinkLog(); + _logRunning = true; + emit logRunningChanged(); + } +} + +//----------------------------------------------------------------------------- +void +MavlinkLogManager::stopLogging() +{ + if(_vehicle) { + _vehicle->stopMavlinkLog(); + _logRunning = false; + emit logRunningChanged(); + } +} + //----------------------------------------------------------------------------- QHttpPart create_form_part(const QString& name, const QString& value) @@ -347,3 +384,60 @@ MavlinkLogManager::_uploadProgress(qint64 bytesSent, qint64 bytesTotal) } qCDebug(MavlinkLogManagerLog) << bytesSent << "of" << bytesTotal; } + +//----------------------------------------------------------------------------- +void +MavlinkLogManager::_activeVehicleChanged(Vehicle* vehicle) +{ + //-- TODO: This is not quite right. This is being used to detect when a vehicle + // connects/disconnects. In reality, if QGC is connected to multiple vehicles, + // this is called each time the user switches from one vehicle to another. So + // far, I'm working on the assumption that multiple vehicles is a rare exception. + + // Disconnect the previous one (if any) + if (_vehicle) { + disconnect(_vehicle, &Vehicle::armedChanged, this, &MavlinkLogManager::_armedChanged); + disconnect(_vehicle, &Vehicle::mavlinkLogData, this, &MavlinkLogManager::_mavlinkLogData); + _vehicle = NULL; + emit canStartLogChanged(); + } + // Connect new system + if (vehicle) + { + _vehicle = vehicle; + connect(_vehicle, &Vehicle::armedChanged, this, &MavlinkLogManager::_armedChanged); + connect(_vehicle, &Vehicle::mavlinkLogData, this, &MavlinkLogManager::_mavlinkLogData); + emit canStartLogChanged(); + } +} + +//----------------------------------------------------------------------------- +void +MavlinkLogManager::_mavlinkLogData(Vehicle * /*vehicle*/, uint8_t /*target_system*/, uint8_t /*target_component*/, uint16_t sequence, uint8_t length, uint8_t first_message, const uint8_t* data, bool /*acked*/) +{ + Q_UNUSED(data); + qDebug() << "Mavlink Log:" << sequence << length << first_message; +} + +//----------------------------------------------------------------------------- +void +MavlinkLogManager::_armedChanged(bool armed) +{ + if(_vehicle) { + if(armed) { + if(_enableAutoStart) { + _vehicle->startMavlinkLog(); + _logRunning = true; + emit logRunningChanged(); + } + } else { + if(_logRunning && _enableAutoStart) { + _vehicle->stopMavlinkLog(); + emit logRunningChanged(); + if(_enableAutoUpload) { + //-- TODO: Queue log for auto upload + } + } + } + } +} diff --git a/src/uas/MavlinkLogManager.h b/src/uas/MavlinkLogManager.h index de41bc1ef..3ef3dc585 100644 --- a/src/uas/MavlinkLogManager.h +++ b/src/uas/MavlinkLogManager.h @@ -16,6 +16,7 @@ #include "QmlObjectListModel.h" #include "QGCLoggingCategory.h" #include "QGCToolbox.h" +#include "Vehicle.h" Q_DECLARE_LOGGING_CATEGORY(MavlinkLogManagerLog) @@ -67,29 +68,38 @@ public: MavlinkLogManager (QGCApplication* app); ~MavlinkLogManager (); - Q_PROPERTY(QString emailAddress READ emailAddress WRITE setEmailAddress NOTIFY emailAddressChanged) - Q_PROPERTY(QString description READ description WRITE setDescription NOTIFY descriptionChanged) - Q_PROPERTY(QString uploadURL READ uploadURL WRITE setUploadURL NOTIFY uploadURLChanged) - Q_PROPERTY(bool enableAutolog READ enableAutolog WRITE setEnableAutolog NOTIFY enableAutologChanged) - Q_PROPERTY(bool busy READ busy NOTIFY busyChanged) - Q_PROPERTY(QmlObjectListModel* logFiles READ logFiles NOTIFY logFilesChanged) + Q_PROPERTY(QString emailAddress READ emailAddress WRITE setEmailAddress NOTIFY emailAddressChanged) + Q_PROPERTY(QString description READ description WRITE setDescription NOTIFY descriptionChanged) + Q_PROPERTY(QString uploadURL READ uploadURL WRITE setUploadURL NOTIFY uploadURLChanged) + Q_PROPERTY(bool enableAutoUpload READ enableAutoUpload WRITE setEnableAutoUpload NOTIFY enableAutoUploadChanged) + Q_PROPERTY(bool enableAutoStart READ enableAutoStart WRITE setEnableAutoStart NOTIFY enableAutoStartChanged) + Q_PROPERTY(bool busy READ busy NOTIFY busyChanged) + Q_PROPERTY(bool logRunning READ logRunning NOTIFY logRunningChanged) + Q_PROPERTY(bool canStartLog READ canStartLog NOTIFY canStartLogChanged) + Q_PROPERTY(QmlObjectListModel* logFiles READ logFiles NOTIFY logFilesChanged) Q_INVOKABLE void uploadLog (); Q_INVOKABLE void deleteLog (); Q_INVOKABLE void cancelUpload (); + Q_INVOKABLE void startLogging (); + Q_INVOKABLE void stopLogging (); QString emailAddress () { return _emailAddress; } QString description () { return _description; } QString uploadURL () { return _uploadURL; } - bool enableAutolog () { return _enableAutolog; } + bool enableAutoUpload () { return _enableAutoUpload; } + bool enableAutoStart () { return _enableAutoStart; } bool busy (); + bool logRunning () { return _logRunning; } + bool canStartLog () { return _vehicle != NULL; } QmlObjectListModel* logFiles () { return &_logFiles; } void setEmailAddress (QString email); void setDescription (QString description); void setUploadURL (QString url); - void setEnableAutolog (bool enable); + void setEnableAutoUpload (bool enable); + void setEnableAutoStart (bool enable); // Override from QGCTool void setToolbox (QGCToolbox *toolbox); @@ -98,7 +108,8 @@ signals: void emailAddressChanged (); void descriptionChanged (); void uploadURLChanged (); - void enableAutologChanged (); + void enableAutoUploadChanged (); + void enableAutoStartChanged (); void logFilesChanged (); void selectedCountChanged (); void busyChanged (); @@ -106,11 +117,16 @@ signals: void failed (); void succeed (); void abortUpload (); + void logRunningChanged (); + void canStartLogChanged (); private slots: void _uploadFinished (); void _dataAvailable (); void _uploadProgress (qint64 bytesSent, qint64 bytesTotal); + void _activeVehicleChanged (Vehicle* vehicle); + void _mavlinkLogData (Vehicle* vehicle, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t length, uint8_t first_message, const uint8_t* data, bool acked); + void _armedChanged (bool armed); private: bool _sendLog (const QString& logFile); @@ -121,10 +137,13 @@ private: QString _emailAddress; QString _uploadURL; QString _logPath; - bool _enableAutolog; + bool _enableAutoUpload; + bool _enableAutoStart; QNetworkAccessManager* _nam; QmlObjectListModel _logFiles; MavlinkLogFiles* _currentLogfile; + Vehicle* _vehicle; + bool _logRunning; }; #endif diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index cf022f593..10ad42fe8 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -307,6 +307,7 @@ signals: // Log Download Signals void logEntry (UASInterface* uas, uint32_t time_utc, uint32_t size, uint16_t id, uint16_t num_logs, uint16_t last_log_num); void logData (UASInterface* uas, uint32_t ofs, uint16_t id, uint8_t count, const uint8_t* data); + }; Q_DECLARE_INTERFACE(UASInterface, "org.qgroundcontrol/1.0") diff --git a/src/ui/preferences/MavlinkSettings.qml b/src/ui/preferences/MavlinkSettings.qml index 9f0cf1e40..b73fc5620 100644 --- a/src/ui/preferences/MavlinkSettings.qml +++ b/src/ui/preferences/MavlinkSettings.qml @@ -130,6 +130,57 @@ Rectangle { } //----------------------------------------------------------------- //-- Mavlink Logging + Item { + width: __mavlinkRoot.width * 0.8 + height: mavlogLabel.height + anchors.margins: ScreenTools.defaultFontPixelWidth + anchors.horizontalCenter: parent.horizontalCenter + QGCLabel { + id: mavlogLabel + text: qsTr("Vehicle Mavlink Logging") + font.family: ScreenTools.demiboldFontFamily + } + } + Rectangle { + height: mavlogColumn.height + (ScreenTools.defaultFontPixelHeight * 2) + width: __mavlinkRoot.width * 0.8 + color: qgcPal.windowShade + anchors.margins: ScreenTools.defaultFontPixelWidth + anchors.horizontalCenter: parent.horizontalCenter + Column { + id: mavlogColumn + width: gcsColumn.width + spacing: ScreenTools.defaultFontPixelWidth + anchors.centerIn: parent + //----------------------------------------------------------------- + //-- Enable auto log on arming + QGCCheckBox { + text: qsTr("Enable automatic logging start when vehicle is armed") + checked: QGroundControl.mavlinkLogManager.enableAutoStart + onClicked: { + QGroundControl.mavlinkLogManager.enableAutoStart = checked + } + } + //----------------------------------------------------------------- + //-- Manual Start/Stop + Row { + spacing: ScreenTools.defaultFontPixelWidth + anchors.horizontalCenter: parent.horizontalCenter + QGCButton { + text: "Start Logging" + enabled: !QGroundControl.mavlinkLogManager.logRunning && QGroundControl.mavlinkLogManager.canStartLog + onClicked: QGroundControl.mavlinkLogManager.startLogging() + } + QGCButton { + text: "Stop Logging" + enabled: QGroundControl.mavlinkLogManager.logRunning + onClicked: QGroundControl.mavlinkLogManager.stopLogging() + } + } + } + } + //----------------------------------------------------------------- + //-- Mavlink Logging Item { width: __mavlinkRoot.width * 0.8 height: logLabel.height @@ -137,7 +188,7 @@ Rectangle { anchors.horizontalCenter: parent.horizontalCenter QGCLabel { id: logLabel - text: qsTr("Vehicle Mavlink Logging") + text: qsTr("Mavlink Log Uploads") font.family: ScreenTools.demiboldFontFamily } } @@ -214,10 +265,10 @@ Rectangle { //-- Automatic Upload QGCCheckBox { text: qsTr("Enable automatic log uploads") - checked: QGroundControl.mavlinkLogManager.enableAutolog + checked: QGroundControl.mavlinkLogManager.enableAutoUpload enabled: emailField.text !== "" && urlField !== "" onClicked: { - QGroundControl.mavlinkLogManager.enableAutolog = checked + QGroundControl.mavlinkLogManager.enableAutoUpload = checked } } } -- 2.22.0