Commit 93ee9ec2 authored by Gus Grubba's avatar Gus Grubba Committed by GitHub

Merge pull request #4180 from dogmaphobic/MavlinkLogUploader

Mavlink log uploader
parents 5c94cda4 c77d8df1
......@@ -334,6 +334,7 @@ HEADERS += \
src/uas/UAS.h \
src/uas/UASInterface.h \
src/uas/UASMessageHandler.h \
src/Vehicle/MavlinkLogManager.h \
src/ui/toolbar/MainToolBarController.h \
src/AutoPilotPlugins/PX4/PX4AirframeLoader.h \
src/AutoPilotPlugins/APM/APMAirframeLoader.h \
......@@ -498,6 +499,7 @@ SOURCES += \
src/QmlControls/QmlObjectListModel.cc \
src/uas/UAS.cc \
src/uas/UASMessageHandler.cc \
src/Vehicle/MavlinkLogManager.cc \
src/ui/toolbar/MainToolBarController.cc \
src/AutoPilotPlugins/PX4/PX4AirframeLoader.cc \
src/AutoPilotPlugins/APM/APMAirframeLoader.cc \
......
......@@ -28,6 +28,7 @@
#include "FollowMe.h"
#include "PositionManager.h"
#include "VideoManager.h"
#include "MavlinkLogManager.h"
QGCToolbox::QGCToolbox(QGCApplication* app)
: _audioOutput(NULL)
......@@ -50,6 +51,7 @@ QGCToolbox::QGCToolbox(QGCApplication* app)
, _followMe(NULL)
, _qgcPositionManager(NULL)
, _videoManager(NULL)
, _mavlinkLogManager(NULL)
{
_audioOutput = new GAudioOutput(app);
_autopilotPluginManager = new AutoPilotPluginManager(app);
......@@ -71,6 +73,7 @@ QGCToolbox::QGCToolbox(QGCApplication* app)
_qgcPositionManager = new QGCPositionManager(app);
_followMe = new FollowMe(app);
_videoManager = new VideoManager(app);
_mavlinkLogManager = new MavlinkLogManager(app);
}
void QGCToolbox::setChildToolboxes(void)
......@@ -95,11 +98,13 @@ void QGCToolbox::setChildToolboxes(void)
_followMe->setToolbox(this);
_qgcPositionManager->setToolbox(this);
_videoManager->setToolbox(this);
_mavlinkLogManager->setToolbox(this);
}
QGCToolbox::~QGCToolbox()
{
delete _videoManager;
delete _mavlinkLogManager;
delete _audioOutput;
delete _autopilotPluginManager;
delete _factSystem;
......
......@@ -32,6 +32,7 @@ class QGCImageProvider;
class UASMessageHandler;
class QGCPositionManager;
class VideoManager;
class MavlinkLogManager;
/// This is used to manage all of our top level services/tools
class QGCToolbox {
......@@ -56,6 +57,8 @@ public:
FollowMe* followMe(void) { return _followMe; }
QGCPositionManager* qgcPositionManager(void) { return _qgcPositionManager; }
VideoManager* videoManager(void) { return _videoManager; }
MavlinkLogManager* mavlinkLogManager(void) { return _mavlinkLogManager; }
#ifndef __mobile__
GPSManager* gpsManager(void) { return _gpsManager; }
#endif
......@@ -83,6 +86,7 @@ private:
FollowMe* _followMe;
QGCPositionManager* _qgcPositionManager;
VideoManager* _videoManager;
MavlinkLogManager* _mavlinkLogManager;
friend class QGCApplication;
};
......
......@@ -44,6 +44,7 @@ QGroundControlQmlGlobal::QGroundControlQmlGlobal(QGCApplication* app)
, _qgcPositionManager(NULL)
, _missionCommandTree(NULL)
, _videoManager(NULL)
, _mavlinkLogManager(NULL)
, _virtualTabletJoystick(false)
, _baseFontPointSize(0.0)
{
......@@ -60,7 +61,6 @@ QGroundControlQmlGlobal::~QGroundControlQmlGlobal()
}
void QGroundControlQmlGlobal::setToolbox(QGCToolbox* toolbox)
{
QGCTool::setToolbox(toolbox);
......@@ -72,9 +72,9 @@ void QGroundControlQmlGlobal::setToolbox(QGCToolbox* toolbox)
_qgcPositionManager = toolbox->qgcPositionManager();
_missionCommandTree = toolbox->missionCommandTree();
_videoManager = toolbox->videoManager();
_mavlinkLogManager = toolbox->mavlinkLogManager();
}
void QGroundControlQmlGlobal::saveGlobalSetting (const QString& key, const QString& value)
{
QSettings settings;
......
......@@ -72,6 +72,7 @@ public:
Q_PROPERTY(QGCPositionManager* qgcPositionManger READ qgcPositionManger CONSTANT)
Q_PROPERTY(MissionCommandTree* missionCommandTree READ missionCommandTree CONSTANT)
Q_PROPERTY(VideoManager* videoManager READ videoManager CONSTANT)
Q_PROPERTY(MavlinkLogManager* mavlinkLogManager READ mavlinkLogManager CONSTANT)
Q_PROPERTY(qreal zOrderTopMost READ zOrderTopMost CONSTANT) ///< z order for top most items, toolbar, main window sub view
Q_PROPERTY(qreal zOrderWidgets READ zOrderWidgets CONSTANT) ///< z order value to widgets, for example: zoom controls, hud widgetss
......@@ -166,6 +167,7 @@ public:
QGCPositionManager* qgcPositionManger () { return _qgcPositionManager; }
MissionCommandTree* missionCommandTree () { return _missionCommandTree; }
VideoManager* videoManager () { return _videoManager; }
MavlinkLogManager* mavlinkLogManager () { return _mavlinkLogManager; }
qreal zOrderTopMost () { return 1000; }
qreal zOrderWidgets () { return 100; }
......@@ -237,6 +239,7 @@ private:
QGCPositionManager* _qgcPositionManager;
MissionCommandTree* _missionCommandTree;
VideoManager* _videoManager;
MavlinkLogManager* _mavlinkLogManager;
bool _virtualTabletJoystick;
qreal _baseFontPointSize;
......
This diff is collapsed.
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#ifndef MavlinkLogManager_H
#define MavlinkLogManager_H
#include <QObject>
#include "QmlObjectListModel.h"
#include "QGCLoggingCategory.h"
#include "QGCToolbox.h"
#include "Vehicle.h"
Q_DECLARE_LOGGING_CATEGORY(MavlinkLogManagerLog)
class QNetworkAccessManager;
class MavlinkLogManager;
//-----------------------------------------------------------------------------
class MavlinkLogFiles : public QObject
{
Q_OBJECT
public:
MavlinkLogFiles (MavlinkLogManager* manager, const QString& filePath, bool newFile = false);
Q_PROPERTY(QString name READ name CONSTANT)
Q_PROPERTY(quint32 size READ size NOTIFY sizeChanged)
Q_PROPERTY(bool selected READ selected WRITE setSelected NOTIFY selectedChanged)
Q_PROPERTY(bool uploading READ uploading NOTIFY uploadingChanged)
Q_PROPERTY(qreal progress READ progress NOTIFY progressChanged)
Q_PROPERTY(bool writing READ writing NOTIFY writingChanged)
Q_PROPERTY(bool uploaded READ uploaded NOTIFY uploadedChanged)
QString name () { return _name; }
quint32 size () { return _size; }
bool selected () { return _selected; }
bool uploading () { return _uploading; }
qreal progress () { return _progress; }
bool writing () { return _writing; }
bool uploaded () { return _uploaded; }
void setSelected (bool selected);
void setUploading (bool uploading);
void setProgress (qreal progress);
void setWriting (bool writing);
void setSize (quint32 size);
void setUploaded (bool uploaded);
signals:
void sizeChanged ();
void selectedChanged ();
void uploadingChanged ();
void progressChanged ();
void writingChanged ();
void uploadedChanged ();
private:
MavlinkLogManager* _manager;
QString _name;
quint32 _size;
bool _selected;
bool _uploading;
qreal _progress;
bool _writing;
bool _uploaded;
};
//-----------------------------------------------------------------------------
class MavlinkLogProcessor
{
public:
MavlinkLogProcessor();
~MavlinkLogProcessor();
void close ();
bool valid ();
bool create (MavlinkLogManager *manager, const QString path, uint8_t id);
MavlinkLogFiles* record () { return _record; }
QString fileName () { return _fileName; }
bool processStreamData(uint16_t _sequence, uint8_t first_message, QByteArray data);
private:
bool _checkSequence(uint16_t seq, int &num_drops);
QByteArray _writeUlogMessage(QByteArray &data);
void _writeData(void* data, int len);
private:
FILE* _fd;
quint32 _written;
int _sequence;
int _numDrops;
bool _gotHeader;
bool _error;
QByteArray _ulogMessage;
QString _fileName;
MavlinkLogFiles* _record;
};
//-----------------------------------------------------------------------------
class MavlinkLogManager : public QGCTool
{
Q_OBJECT
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 enableAutoUpload READ enableAutoUpload WRITE setEnableAutoUpload NOTIFY enableAutoUploadChanged)
Q_PROPERTY(bool enableAutoStart READ enableAutoStart WRITE setEnableAutoStart NOTIFY enableAutoStartChanged)
Q_PROPERTY(bool deleteAfterUpload READ deleteAfterUpload WRITE setDeleteAfterUpload NOTIFY deleteAfterUploadChanged)
Q_PROPERTY(bool uploading READ uploading NOTIFY uploadingChanged)
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 enableAutoUpload () { return _enableAutoUpload; }
bool enableAutoStart () { return _enableAutoStart; }
bool uploading ();
bool logRunning () { return _logRunning; }
bool canStartLog () { return _vehicle != NULL; }
bool deleteAfterUpload () { return _deleteAfterUpload; }
QmlObjectListModel* logFiles () { return &_logFiles; }
void setEmailAddress (QString email);
void setDescription (QString description);
void setUploadURL (QString url);
void setEnableAutoUpload (bool enable);
void setEnableAutoStart (bool enable);
void setDeleteAfterUpload(bool enable);
// Override from QGCTool
void setToolbox (QGCToolbox *toolbox);
signals:
void emailAddressChanged ();
void descriptionChanged ();
void uploadURLChanged ();
void enableAutoUploadChanged ();
void enableAutoStartChanged ();
void logFilesChanged ();
void selectedCountChanged ();
void uploadingChanged ();
void readyRead (QByteArray data);
void failed ();
void succeed ();
void abortUpload ();
void logRunningChanged ();
void canStartLogChanged ();
void deleteAfterUploadChanged ();
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 first_message, QByteArray data, bool acked);
void _armedChanged (bool armed);
void _commandLongAck (uint8_t compID, uint16_t command, uint8_t result);
void _processCmdAck ();
private:
bool _sendLog (const QString& logFile);
bool _processUploadResponse (int http_code, QByteArray &data);
bool _createNewLog ();
int _getFirstSelected ();
void _insertNewLog (MavlinkLogFiles* newLog);
void _deleteLog (MavlinkLogFiles* log);
void _discardLog ();
QString _makeFilename (const QString& baseName);
private:
QString _description;
QString _emailAddress;
QString _uploadURL;
QString _logPath;
bool _enableAutoUpload;
bool _enableAutoStart;
QNetworkAccessManager* _nam;
QmlObjectListModel _logFiles;
MavlinkLogFiles* _currentLogfile;
Vehicle* _vehicle;
bool _logRunning;
bool _loggingDisabled;
MavlinkLogProcessor* _logProcessor;
bool _deleteAfterUpload;
int _loggingCmdTryCount;
QTimer _ackTimer;
};
#endif
......@@ -400,6 +400,7 @@ Vehicle::resetCounters()
void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message)
{
if (message.sysid != _id && message.sysid != 0) {
return;
}
......@@ -480,7 +481,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
_handleCommandAck(message);
break;
case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
_handleAutopilotVersion(message);
_handleAutopilotVersion(link, message);
break;
case MAVLINK_MSG_ID_WIND_COV:
_handleWindCov(message);
......@@ -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
......@@ -501,11 +508,17 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
_uas->receiveMessage(message);
}
void Vehicle::_handleAutopilotVersion(mavlink_message_t& message)
void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& message)
{
mavlink_autopilot_version_t autopilotVersion;
mavlink_msg_autopilot_version_decode(&message, &autopilotVersion);
bool isMavlink2 = (autopilotVersion.capabilities & MAV_PROTOCOL_CAPABILITY_MAVLINK2) != 0;
if(isMavlink2) {
mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(link->mavlinkChannel());
mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
}
if (autopilotVersion.flight_sw_version != 0) {
int majorVersion, minorVersion, patchVersion;
FIRMWARE_VERSION_TYPE versionType;
......@@ -549,9 +562,14 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message)
emit commandLongAck(message.compid, ack.command, ack.result);
if (ack.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
// Disregard failures
return;
// Disregard failures for these (handled above)
switch (ack.command) {
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
case MAV_CMD_LOGGING_START:
case MAV_CMD_LOGGING_STOP:
return;
default:
break;
}
QString commandName = qgcApp()->toolbox()->missionCommandTree()->friendlyName((MAV_CMD)ack.command);
......@@ -1959,6 +1977,62 @@ VehicleGPSFactGroup::VehicleGPSFactGroup(QObject* parent)
_courseOverGroundFact.setRawValue(std::numeric_limits<float>::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)
{
mavlink_logging_data_t log;
mavlink_msg_logging_data_decode(&message, &log);
emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence,
log.first_message_offset, QByteArray((const char*)log.data, log.length), false);
}
//-----------------------------------------------------------------------------
void
Vehicle::_handleMavlinkLoggingDataAcked(mavlink_message_t& message)
{
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.first_message_offset, QByteArray((const char*)log.data, log.length), true);
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
void VehicleGPSFactGroup::setVehicle(Vehicle* vehicle)
{
_vehicle = vehicle;
......
......@@ -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 first_message, QByteArray data, bool acked);
private slots:
void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message);
void _linkInactiveOrDeleted(LinkInterface* link);
......@@ -685,7 +692,7 @@ private:
void _handleVibration(mavlink_message_t& message);
void _handleExtendedSysState(mavlink_message_t& message);
void _handleCommandAck(mavlink_message_t& message);
void _handleAutopilotVersion(mavlink_message_t& message);
void _handleAutopilotVersion(LinkInterface* link, mavlink_message_t& message);
void _handleHilActuatorControls(mavlink_message_t& message);
void _missionManagerError(int errorCode, const QString& errorMsg);
void _geoFenceManagerError(int errorCode, const QString& errorMsg);
......@@ -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
......
......@@ -73,7 +73,7 @@ MAVLinkProtocol::MAVLinkProtocol(QGCApplication* app)
MAVLinkProtocol::~MAVLinkProtocol()
{
storeSettings();
#ifndef __mobile__
_closeLogFile();
#endif
......@@ -162,7 +162,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
if (!_linkMgr->links()->contains(link)) {
return;
}
// receiveMutex.lock();
mavlink_message_t message;
mavlink_status_t status;
......@@ -214,6 +214,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
{
decodedFirstPacket = true;
/*
* Handled in Vehicle.cc now.
mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel);
if (!(mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) && (mavlinkStatus->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) {
qDebug() << "switch to mavlink 2.0" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags;
......@@ -222,6 +224,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
qDebug() << "switch to mavlink 1.0" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags;
mavlinkStatus->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
}
*/
if(message.msgid == MAVLINK_MSG_ID_RADIO_STATUS)
{
......@@ -255,7 +258,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
#ifndef __mobile__
// Log data
if (!_logSuspendError && !_logSuspendReplay && _tempLogFile.isOpen()) {
uint8_t buf[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)];
......@@ -280,7 +283,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
_stopLogging();
_logSuspendError = true;
}
// Check for the vehicle arming going by. This is used to trigger log save.
if (!_logPromptForSave && message.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
mavlink_heartbeat_t state;
......@@ -412,7 +415,7 @@ bool MAVLinkProtocol::_closeLogFile(void)
return true;
}
}
return false;
}
......@@ -458,11 +461,11 @@ void MAVLinkProtocol::_stopLogging(void)
void MAVLinkProtocol::checkForLostLogFiles(void)
{
QDir tempDir(QStandardPaths::writableLocation(QStandardPaths::TempLocation));
QString filter(QString("*.%1").arg(_logFileExtension));
QFileInfoList fileInfoList = tempDir.entryInfoList(QStringList(filter), QDir::Files);
qDebug() << "Orphaned log file count" << fileInfoList.count();
foreach(const QFileInfo fileInfo, fileInfoList) {
qDebug() << "Orphaned log file" << fileInfo.filePath();
if (fileInfo.size() == 0) {
......@@ -488,10 +491,10 @@ void MAVLinkProtocol::suspendLogForReplay(bool suspend)
void MAVLinkProtocol::deleteTempLogFiles(void)
{
QDir tempDir(QStandardPaths::writableLocation(QStandardPaths::TempLocation));
QString filter(QString("*.%1").arg(_logFileExtension));
QFileInfoList fileInfoList = tempDir.entryInfoList(QStringList(filter), QDir::Files);
foreach(const QFileInfo fileInfo, fileInfoList) {
QFile::remove(fileInfo.filePath());
}
......
......@@ -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")
......
This diff is collapsed.
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment