Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Q
qgroundcontrol
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
4d23912a
Commit
4d23912a
authored
Oct 25, 2016
by
Gus Grubba
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Receiving and writing logs.
Forcing QGC to use Mavlink V2 if vehicle supports it.
parent
f91c5bef
Changes
6
Hide whitespace changes
Inline
Side-by-side
Showing
6 changed files
with
168 additions
and
74 deletions
+168
-74
Vehicle.cc
src/Vehicle/Vehicle.cc
+8
-4
Vehicle.h
src/Vehicle/Vehicle.h
+1
-1
MAVLinkProtocol.cc
src/comm/MAVLinkProtocol.cc
+12
-9
MavlinkLogManager.cc
src/uas/MavlinkLogManager.cc
+130
-48
MavlinkLogManager.h
src/uas/MavlinkLogManager.h
+6
-0
MavlinkSettings.qml
src/ui/preferences/MavlinkSettings.qml
+11
-12
No files found.
src/Vehicle/Vehicle.cc
View file @
4d23912a
...
@@ -481,7 +481,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
...
@@ -481,7 +481,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
_handleCommandAck
(
message
);
_handleCommandAck
(
message
);
break
;
break
;
case
MAVLINK_MSG_ID_AUTOPILOT_VERSION
:
case
MAVLINK_MSG_ID_AUTOPILOT_VERSION
:
_handleAutopilotVersion
(
message
);
_handleAutopilotVersion
(
link
,
message
);
break
;
break
;
case
MAVLINK_MSG_ID_WIND_COV
:
case
MAVLINK_MSG_ID_WIND_COV
:
_handleWindCov
(
message
);
_handleWindCov
(
message
);
...
@@ -508,11 +508,17 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
...
@@ -508,11 +508,17 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
_uas
->
receiveMessage
(
message
);
_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_autopilot_version_t
autopilotVersion
;
mavlink_msg_autopilot_version_decode
(
&
message
,
&
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
)
{
if
(
autopilotVersion
.
flight_sw_version
!=
0
)
{
int
majorVersion
,
minorVersion
,
patchVersion
;
int
majorVersion
,
minorVersion
,
patchVersion
;
FIRMWARE_VERSION_TYPE
versionType
;
FIRMWARE_VERSION_TYPE
versionType
;
...
@@ -2002,7 +2008,6 @@ Vehicle::_ackMavlinkLogData(uint16_t sequence)
...
@@ -2002,7 +2008,6 @@ Vehicle::_ackMavlinkLogData(uint16_t sequence)
void
void
Vehicle
::
_handleMavlinkLoggingData
(
mavlink_message_t
&
message
)
Vehicle
::
_handleMavlinkLoggingData
(
mavlink_message_t
&
message
)
{
{
qDebug
()
<<
"MAVLINK_MSG_ID_LOGGING_DATA"
;
mavlink_logging_data_t
log
;
mavlink_logging_data_t
log
;
mavlink_msg_logging_data_decode
(
&
message
,
&
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
);
emit
mavlinkLogData
(
this
,
log
.
target_system
,
log
.
target_component
,
log
.
sequence
,
log
.
length
,
log
.
first_message_offset
,
log
.
data
,
false
);
...
@@ -2012,7 +2017,6 @@ Vehicle::_handleMavlinkLoggingData(mavlink_message_t& message)
...
@@ -2012,7 +2017,6 @@ Vehicle::_handleMavlinkLoggingData(mavlink_message_t& message)
void
void
Vehicle
::
_handleMavlinkLoggingDataAcked
(
mavlink_message_t
&
message
)
Vehicle
::
_handleMavlinkLoggingDataAcked
(
mavlink_message_t
&
message
)
{
{
qDebug
()
<<
"MAVLINK_MSG_ID_LOGGING_DATA_ACKED"
;
mavlink_logging_data_t
log
;
mavlink_logging_data_t
log
;
mavlink_msg_logging_data_decode
(
&
message
,
&
log
);
mavlink_msg_logging_data_decode
(
&
message
,
&
log
);
_ackMavlinkLogData
(
log
.
sequence
);
_ackMavlinkLogData
(
log
.
sequence
);
...
...
src/Vehicle/Vehicle.h
View file @
4d23912a
...
@@ -692,7 +692,7 @@ private:
...
@@ -692,7 +692,7 @@ private:
void
_handleVibration
(
mavlink_message_t
&
message
);
void
_handleVibration
(
mavlink_message_t
&
message
);
void
_handleExtendedSysState
(
mavlink_message_t
&
message
);
void
_handleExtendedSysState
(
mavlink_message_t
&
message
);
void
_handleCommandAck
(
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
_handleHilActuatorControls
(
mavlink_message_t
&
message
);
void
_missionManagerError
(
int
errorCode
,
const
QString
&
errorMsg
);
void
_missionManagerError
(
int
errorCode
,
const
QString
&
errorMsg
);
void
_geoFenceManagerError
(
int
errorCode
,
const
QString
&
errorMsg
);
void
_geoFenceManagerError
(
int
errorCode
,
const
QString
&
errorMsg
);
...
...
src/comm/MAVLinkProtocol.cc
View file @
4d23912a
...
@@ -73,7 +73,7 @@ MAVLinkProtocol::MAVLinkProtocol(QGCApplication* app)
...
@@ -73,7 +73,7 @@ MAVLinkProtocol::MAVLinkProtocol(QGCApplication* app)
MAVLinkProtocol
::~
MAVLinkProtocol
()
MAVLinkProtocol
::~
MAVLinkProtocol
()
{
{
storeSettings
();
storeSettings
();
#ifndef __mobile__
#ifndef __mobile__
_closeLogFile
();
_closeLogFile
();
#endif
#endif
...
@@ -162,7 +162,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
...
@@ -162,7 +162,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
if
(
!
_linkMgr
->
links
()
->
contains
(
link
))
{
if
(
!
_linkMgr
->
links
()
->
contains
(
link
))
{
return
;
return
;
}
}
// receiveMutex.lock();
// receiveMutex.lock();
mavlink_message_t
message
;
mavlink_message_t
message
;
mavlink_status_t
status
;
mavlink_status_t
status
;
...
@@ -214,6 +214,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
...
@@ -214,6 +214,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
{
{
decodedFirstPacket
=
true
;
decodedFirstPacket
=
true
;
/*
* Handled in Vehicle.cc now.
mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel);
mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel);
if (!(mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) && (mavlinkStatus->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) {
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;
qDebug() << "switch to mavlink 2.0" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags;
...
@@ -222,6 +224,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
...
@@ -222,6 +224,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
qDebug() << "switch to mavlink 1.0" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags;
qDebug() << "switch to mavlink 1.0" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags;
mavlinkStatus->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
mavlinkStatus->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
}
}
*/
if
(
message
.
msgid
==
MAVLINK_MSG_ID_RADIO_STATUS
)
if
(
message
.
msgid
==
MAVLINK_MSG_ID_RADIO_STATUS
)
{
{
...
@@ -255,7 +258,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
...
@@ -255,7 +258,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
#ifndef __mobile__
#ifndef __mobile__
// Log data
// Log data
if
(
!
_logSuspendError
&&
!
_logSuspendReplay
&&
_tempLogFile
.
isOpen
())
{
if
(
!
_logSuspendError
&&
!
_logSuspendReplay
&&
_tempLogFile
.
isOpen
())
{
uint8_t
buf
[
MAVLINK_MAX_PACKET_LEN
+
sizeof
(
quint64
)];
uint8_t
buf
[
MAVLINK_MAX_PACKET_LEN
+
sizeof
(
quint64
)];
...
@@ -280,7 +283,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
...
@@ -280,7 +283,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
_stopLogging
();
_stopLogging
();
_logSuspendError
=
true
;
_logSuspendError
=
true
;
}
}
// Check for the vehicle arming going by. This is used to trigger log save.
// Check for the vehicle arming going by. This is used to trigger log save.
if
(
!
_logPromptForSave
&&
message
.
msgid
==
MAVLINK_MSG_ID_HEARTBEAT
)
{
if
(
!
_logPromptForSave
&&
message
.
msgid
==
MAVLINK_MSG_ID_HEARTBEAT
)
{
mavlink_heartbeat_t
state
;
mavlink_heartbeat_t
state
;
...
@@ -412,7 +415,7 @@ bool MAVLinkProtocol::_closeLogFile(void)
...
@@ -412,7 +415,7 @@ bool MAVLinkProtocol::_closeLogFile(void)
return
true
;
return
true
;
}
}
}
}
return
false
;
return
false
;
}
}
...
@@ -458,11 +461,11 @@ void MAVLinkProtocol::_stopLogging(void)
...
@@ -458,11 +461,11 @@ void MAVLinkProtocol::_stopLogging(void)
void
MAVLinkProtocol
::
checkForLostLogFiles
(
void
)
void
MAVLinkProtocol
::
checkForLostLogFiles
(
void
)
{
{
QDir
tempDir
(
QStandardPaths
::
writableLocation
(
QStandardPaths
::
TempLocation
));
QDir
tempDir
(
QStandardPaths
::
writableLocation
(
QStandardPaths
::
TempLocation
));
QString
filter
(
QString
(
"*.%1"
).
arg
(
_logFileExtension
));
QString
filter
(
QString
(
"*.%1"
).
arg
(
_logFileExtension
));
QFileInfoList
fileInfoList
=
tempDir
.
entryInfoList
(
QStringList
(
filter
),
QDir
::
Files
);
QFileInfoList
fileInfoList
=
tempDir
.
entryInfoList
(
QStringList
(
filter
),
QDir
::
Files
);
qDebug
()
<<
"Orphaned log file count"
<<
fileInfoList
.
count
();
qDebug
()
<<
"Orphaned log file count"
<<
fileInfoList
.
count
();
foreach
(
const
QFileInfo
fileInfo
,
fileInfoList
)
{
foreach
(
const
QFileInfo
fileInfo
,
fileInfoList
)
{
qDebug
()
<<
"Orphaned log file"
<<
fileInfo
.
filePath
();
qDebug
()
<<
"Orphaned log file"
<<
fileInfo
.
filePath
();
if
(
fileInfo
.
size
()
==
0
)
{
if
(
fileInfo
.
size
()
==
0
)
{
...
@@ -488,10 +491,10 @@ void MAVLinkProtocol::suspendLogForReplay(bool suspend)
...
@@ -488,10 +491,10 @@ void MAVLinkProtocol::suspendLogForReplay(bool suspend)
void
MAVLinkProtocol
::
deleteTempLogFiles
(
void
)
void
MAVLinkProtocol
::
deleteTempLogFiles
(
void
)
{
{
QDir
tempDir
(
QStandardPaths
::
writableLocation
(
QStandardPaths
::
TempLocation
));
QDir
tempDir
(
QStandardPaths
::
writableLocation
(
QStandardPaths
::
TempLocation
));
QString
filter
(
QString
(
"*.%1"
).
arg
(
_logFileExtension
));
QString
filter
(
QString
(
"*.%1"
).
arg
(
_logFileExtension
));
QFileInfoList
fileInfoList
=
tempDir
.
entryInfoList
(
QStringList
(
filter
),
QDir
::
Files
);
QFileInfoList
fileInfoList
=
tempDir
.
entryInfoList
(
QStringList
(
filter
),
QDir
::
Files
);
foreach
(
const
QFileInfo
fileInfo
,
fileInfoList
)
{
foreach
(
const
QFileInfo
fileInfo
,
fileInfoList
)
{
QFile
::
remove
(
fileInfo
.
filePath
());
QFile
::
remove
(
fileInfo
.
filePath
());
}
}
...
...
src/uas/MavlinkLogManager.cc
View file @
4d23912a
...
@@ -30,7 +30,7 @@ static const char* kEnableAutoUploadKey = "EnableAutoUploadKey";
...
@@ -30,7 +30,7 @@ static const char* kEnableAutoUploadKey = "EnableAutoUploadKey";
static
const
char
*
kEnableAutoStartKey
=
"EnableAutoStartKey"
;
static
const
char
*
kEnableAutoStartKey
=
"EnableAutoStartKey"
;
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
MavlinkLogFiles
::
MavlinkLogFiles
(
MavlinkLogManager
*
manager
,
const
QString
&
filePath
)
MavlinkLogFiles
::
MavlinkLogFiles
(
MavlinkLogManager
*
manager
,
const
QString
&
filePath
)
:
_manager
(
manager
)
:
_manager
(
manager
)
,
_size
(
0
)
,
_size
(
0
)
,
_selected
(
false
)
,
_selected
(
false
)
...
@@ -76,6 +76,9 @@ MavlinkLogManager::MavlinkLogManager(QGCApplication* app)
...
@@ -76,6 +76,9 @@ MavlinkLogManager::MavlinkLogManager(QGCApplication* app)
,
_currentLogfile
(
NULL
)
,
_currentLogfile
(
NULL
)
,
_vehicle
(
NULL
)
,
_vehicle
(
NULL
)
,
_logRunning
(
false
)
,
_logRunning
(
false
)
,
_loggingDisabled
(
false
)
,
_currentSavingFileFd
(
NULL
)
,
_sequence
(
0
)
{
{
//-- Get saved settings
//-- Get saved settings
QSettings
settings
;
QSettings
settings
;
...
@@ -90,14 +93,17 @@ MavlinkLogManager::MavlinkLogManager(QGCApplication* app)
...
@@ -90,14 +93,17 @@ MavlinkLogManager::MavlinkLogManager(QGCApplication* app)
if
(
!
QDir
(
_logPath
).
exists
())
{
if
(
!
QDir
(
_logPath
).
exists
())
{
if
(
QDir
().
mkpath
(
_logPath
))
{
if
(
QDir
().
mkpath
(
_logPath
))
{
qCCritical
(
MavlinkLogManagerLog
)
<<
"Could not create Mavlink log download path:"
<<
_logPath
;
qCCritical
(
MavlinkLogManagerLog
)
<<
"Could not create Mavlink log download path:"
<<
_logPath
;
_loggingDisabled
=
true
;
}
}
}
}
//-- Load current list of logs
if
(
!
_loggingDisabled
)
{
QDirIterator
it
(
_logPath
,
QStringList
()
<<
"*.ulg"
,
QDir
::
Files
);
//-- Load current list of logs
while
(
it
.
hasNext
())
{
QDirIterator
it
(
_logPath
,
QStringList
()
<<
"*.ulg"
,
QDir
::
Files
);
_logFiles
.
append
(
new
MavlinkLogFiles
(
this
,
it
.
next
()));
while
(
it
.
hasNext
())
{
_logFiles
.
append
(
new
MavlinkLogFiles
(
this
,
it
.
next
()));
}
qCDebug
(
MavlinkLogManagerLog
)
<<
"Mavlink logs directory:"
<<
_logPath
;
}
}
qCDebug
(
MavlinkLogManagerLog
)
<<
"Mavlink logs directory:"
<<
_logPath
;
}
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
...
@@ -108,19 +114,19 @@ MavlinkLogManager::~MavlinkLogManager()
...
@@ -108,19 +114,19 @@ MavlinkLogManager::~MavlinkLogManager()
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
void
void
MavlinkLogManager
::
setToolbox
(
QGCToolbox
*
toolbox
)
MavlinkLogManager
::
setToolbox
(
QGCToolbox
*
toolbox
)
{
{
QGCTool
::
setToolbox
(
toolbox
);
QGCTool
::
setToolbox
(
toolbox
);
QQmlEngine
::
setObjectOwnership
(
this
,
QQmlEngine
::
CppOwnership
);
QQmlEngine
::
setObjectOwnership
(
this
,
QQmlEngine
::
CppOwnership
);
qmlRegisterUncreatableType
<
MavlinkLogManager
>
(
"QGroundControl.MavlinkLogManager"
,
1
,
0
,
"MavlinkLogManager"
,
"Reference only"
);
qmlRegisterUncreatableType
<
MavlinkLogManager
>
(
"QGroundControl.MavlinkLogManager"
,
1
,
0
,
"MavlinkLogManager"
,
"Reference only"
);
connect
(
toolbox
->
multiVehicleManager
(),
&
MultiVehicleManager
::
activeVehicleChanged
,
this
,
&
MavlinkLogManager
::
_activeVehicleChanged
);
if
(
!
_loggingDisabled
)
{
connect
(
toolbox
->
multiVehicleManager
(),
&
MultiVehicleManager
::
activeVehicleChanged
,
this
,
&
MavlinkLogManager
::
_activeVehicleChanged
);
// _uploadURL = "http://192.168.1.21/px4";
}
// _uploadURL = "http://192.168.1.9:8080
";
// _uploadURL = "http://192.168.1.21/px4
";
// _emailAddress = "gus.grubba.com
";
// _uploadURL = "http://192.168.1.9:8080
";
// _description = "Test from QGroundControl - Discard
";
// _emailAddress = "gus.grubba.com
";
// _sendLog("/Users/gus/github/work/logs/simulator.ulg")
;
// _description = "Test from QGroundControl - Discard"
;
// _sendLog("/Users/gus/github/work/logs/simulator.ulg");
}
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
...
@@ -190,7 +196,7 @@ MavlinkLogManager::uploadLog()
...
@@ -190,7 +196,7 @@ MavlinkLogManager::uploadLog()
if
(
_currentLogfile
)
{
if
(
_currentLogfile
)
{
_currentLogfile
->
setUploading
(
false
);
_currentLogfile
->
setUploading
(
false
);
}
}
for
(
int
i
=
0
;
i
<
_logFiles
.
count
();
i
++
)
{
for
(
int
i
=
0
;
i
<
_logFiles
.
count
();
i
++
)
{
_currentLogfile
=
qobject_cast
<
MavlinkLogFiles
*>
(
_logFiles
.
get
(
i
));
_currentLogfile
=
qobject_cast
<
MavlinkLogFiles
*>
(
_logFiles
.
get
(
i
));
Q_ASSERT
(
_currentLogfile
);
Q_ASSERT
(
_currentLogfile
);
if
(
_currentLogfile
->
selected
())
{
if
(
_currentLogfile
->
selected
())
{
...
@@ -210,18 +216,49 @@ MavlinkLogManager::uploadLog()
...
@@ -210,18 +216,49 @@ MavlinkLogManager::uploadLog()
emit
busyChanged
();
emit
busyChanged
();
}
}
//-----------------------------------------------------------------------------
int
MavlinkLogManager
::
_getFirstSelected
()
{
for
(
int
i
=
0
;
i
<
_logFiles
.
count
();
i
++
)
{
MavlinkLogFiles
*
f
=
qobject_cast
<
MavlinkLogFiles
*>
(
_logFiles
.
get
(
i
));
Q_ASSERT
(
f
);
if
(
f
->
selected
())
{
return
i
;
}
}
return
-
1
;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
void
void
MavlinkLogManager
::
deleteLog
()
MavlinkLogManager
::
deleteLog
()
{
{
//-- TODO
while
(
true
)
{
int
idx
=
_getFirstSelected
();
if
(
idx
<
0
)
{
break
;
}
MavlinkLogFiles
*
f
=
qobject_cast
<
MavlinkLogFiles
*>
(
_logFiles
.
get
(
idx
));
QString
filePath
=
_logPath
;
filePath
+=
"/"
;
filePath
+=
f
->
name
();
filePath
+=
".ulg"
;
QFile
gone
(
filePath
);
if
(
!
gone
.
remove
())
{
qCWarning
(
MavlinkLogManagerLog
)
<<
"Could not delete Mavlink log file:"
<<
_logPath
;
}
_logFiles
.
removeAt
(
idx
);
delete
f
;
emit
logFilesChanged
();
}
}
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
void
void
MavlinkLogManager
::
cancelUpload
()
MavlinkLogManager
::
cancelUpload
()
{
{
for
(
int
i
=
0
;
i
<
_logFiles
.
count
();
i
++
)
{
for
(
int
i
=
0
;
i
<
_logFiles
.
count
();
i
++
)
{
MavlinkLogFiles
*
pLogFile
=
qobject_cast
<
MavlinkLogFiles
*>
(
_logFiles
.
get
(
i
));
MavlinkLogFiles
*
pLogFile
=
qobject_cast
<
MavlinkLogFiles
*>
(
_logFiles
.
get
(
i
));
Q_ASSERT
(
pLogFile
);
Q_ASSERT
(
pLogFile
);
if
(
pLogFile
->
selected
()
&&
pLogFile
!=
_currentLogfile
)
{
if
(
pLogFile
->
selected
()
&&
pLogFile
!=
_currentLogfile
)
{
...
@@ -238,9 +275,11 @@ void
...
@@ -238,9 +275,11 @@ void
MavlinkLogManager
::
startLogging
()
MavlinkLogManager
::
startLogging
()
{
{
if
(
_vehicle
)
{
if
(
_vehicle
)
{
_vehicle
->
startMavlinkLog
();
if
(
_createNewLog
())
{
_logRunning
=
true
;
_vehicle
->
startMavlinkLog
();
emit
logRunningChanged
();
_logRunning
=
true
;
emit
logRunningChanged
();
}
}
}
}
}
...
@@ -252,6 +291,17 @@ MavlinkLogManager::stopLogging()
...
@@ -252,6 +291,17 @@ MavlinkLogManager::stopLogging()
_vehicle
->
stopMavlinkLog
();
_vehicle
->
stopMavlinkLog
();
_logRunning
=
false
;
_logRunning
=
false
;
emit
logRunningChanged
();
emit
logRunningChanged
();
if
(
_currentSavingFileFd
)
{
fclose
(
_currentSavingFileFd
);
_logFiles
.
append
(
new
MavlinkLogFiles
(
this
,
_currentSavingFileStr
));
emit
logFilesChanged
();
_currentSavingFileFd
=
NULL
;
_currentSavingFileStr
.
clear
();
//-- TODO: If auto upload is set, schedule it
if
(
_enableAutoUpload
)
{
//-- Queue log for auto upload
}
}
}
}
}
}
...
@@ -287,21 +337,23 @@ MavlinkLogManager::_sendLog(const QString& logFile)
...
@@ -287,21 +337,23 @@ MavlinkLogManager::_sendLog(const QString& logFile)
qCCritical
(
MavlinkLogManagerLog
)
<<
"Log file missing:"
<<
logFile
;
qCCritical
(
MavlinkLogManagerLog
)
<<
"Log file missing:"
<<
logFile
;
return
false
;
return
false
;
}
}
QFile
*
file
=
new
QFile
(
logFile
);
QFile
*
file
=
new
QFile
(
logFile
);
if
(
!
file
||
!
file
->
open
(
QIODevice
::
ReadOnly
))
{
if
(
!
file
||
!
file
->
open
(
QIODevice
::
ReadOnly
))
{
if
(
file
)
delete
file
;
if
(
file
)
{
delete
file
;
}
qCCritical
(
MavlinkLogManagerLog
)
<<
"Could not open log file:"
<<
logFile
;
qCCritical
(
MavlinkLogManagerLog
)
<<
"Could not open log file:"
<<
logFile
;
return
false
;
return
false
;
}
}
if
(
!
_nam
)
{
if
(
!
_nam
)
{
_nam
=
new
QNetworkAccessManager
(
this
);
_nam
=
new
QNetworkAccessManager
(
this
);
}
}
QNetworkProxy
savedProxy
=
_nam
->
proxy
();
QNetworkProxy
savedProxy
=
_nam
->
proxy
();
QNetworkProxy
tempProxy
;
QNetworkProxy
tempProxy
;
tempProxy
.
setType
(
QNetworkProxy
::
DefaultProxy
);
tempProxy
.
setType
(
QNetworkProxy
::
DefaultProxy
);
_nam
->
setProxy
(
tempProxy
);
_nam
->
setProxy
(
tempProxy
);
//-- Build POST request
//-- Build POST request
QHttpMultiPart
*
multiPart
=
new
QHttpMultiPart
(
QHttpMultiPart
::
FormDataType
);
QHttpMultiPart
*
multiPart
=
new
QHttpMultiPart
(
QHttpMultiPart
::
FormDataType
);
QHttpPart
emailPart
=
create_form_part
(
"email"
,
_emailAddress
);
QHttpPart
emailPart
=
create_form_part
(
"email"
,
_emailAddress
);
QHttpPart
descriptionPart
=
create_form_part
(
"description"
,
_description
);
QHttpPart
descriptionPart
=
create_form_part
(
"description"
,
_description
);
QHttpPart
sourcePart
=
create_form_part
(
"source"
,
"QGroundControl"
);
QHttpPart
sourcePart
=
create_form_part
(
"source"
,
"QGroundControl"
);
...
@@ -319,7 +371,7 @@ MavlinkLogManager::_sendLog(const QString& logFile)
...
@@ -319,7 +371,7 @@ MavlinkLogManager::_sendLog(const QString& logFile)
file
->
setParent
(
multiPart
);
file
->
setParent
(
multiPart
);
QNetworkRequest
request
(
_uploadURL
);
QNetworkRequest
request
(
_uploadURL
);
request
.
setAttribute
(
QNetworkRequest
::
FollowRedirectsAttribute
,
true
);
request
.
setAttribute
(
QNetworkRequest
::
FollowRedirectsAttribute
,
true
);
QNetworkReply
*
reply
=
_nam
->
post
(
request
,
multiPart
);
QNetworkReply
*
reply
=
_nam
->
post
(
request
,
multiPart
);
connect
(
reply
,
&
QNetworkReply
::
finished
,
this
,
&
MavlinkLogManager
::
_uploadFinished
);
connect
(
reply
,
&
QNetworkReply
::
finished
,
this
,
&
MavlinkLogManager
::
_uploadFinished
);
connect
(
this
,
&
MavlinkLogManager
::
abortUpload
,
reply
,
&
QNetworkReply
::
abort
);
connect
(
this
,
&
MavlinkLogManager
::
abortUpload
,
reply
,
&
QNetworkReply
::
abort
);
//connect(reply, &QNetworkReply::readyRead, this, &MavlinkLogManager::_dataAvailable);
//connect(reply, &QNetworkReply::readyRead, this, &MavlinkLogManager::_dataAvailable);
...
@@ -332,7 +384,7 @@ MavlinkLogManager::_sendLog(const QString& logFile)
...
@@ -332,7 +384,7 @@ MavlinkLogManager::_sendLog(const QString& logFile)
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
bool
bool
MavlinkLogManager
::
_processUploadResponse
(
int
http_code
,
QByteArray
&
data
)
MavlinkLogManager
::
_processUploadResponse
(
int
http_code
,
QByteArray
&
data
)
{
{
qCDebug
(
MavlinkLogManagerLog
)
<<
"Uploaded response:"
<<
QString
::
fromUtf8
(
data
);
qCDebug
(
MavlinkLogManagerLog
)
<<
"Uploaded response:"
<<
QString
::
fromUtf8
(
data
);
emit
readyRead
(
data
);
emit
readyRead
(
data
);
...
@@ -343,7 +395,7 @@ MavlinkLogManager::_processUploadResponse(int http_code, QByteArray &data)
...
@@ -343,7 +395,7 @@ MavlinkLogManager::_processUploadResponse(int http_code, QByteArray &data)
void
void
MavlinkLogManager
::
_dataAvailable
()
MavlinkLogManager
::
_dataAvailable
()
{
{
QNetworkReply
*
reply
=
qobject_cast
<
QNetworkReply
*>
(
sender
());
QNetworkReply
*
reply
=
qobject_cast
<
QNetworkReply
*>
(
sender
());
if
(
!
reply
)
{
if
(
!
reply
)
{
return
;
return
;
}
}
...
@@ -355,7 +407,7 @@ MavlinkLogManager::_dataAvailable()
...
@@ -355,7 +407,7 @@ MavlinkLogManager::_dataAvailable()
void
void
MavlinkLogManager
::
_uploadFinished
()
MavlinkLogManager
::
_uploadFinished
()
{
{
QNetworkReply
*
reply
=
qobject_cast
<
QNetworkReply
*>
(
sender
());
QNetworkReply
*
reply
=
qobject_cast
<
QNetworkReply
*>
(
sender
());
if
(
!
reply
)
{
if
(
!
reply
)
{
return
;
return
;
}
}
...
@@ -379,8 +431,9 @@ MavlinkLogManager::_uploadProgress(qint64 bytesSent, qint64 bytesTotal)
...
@@ -379,8 +431,9 @@ MavlinkLogManager::_uploadProgress(qint64 bytesSent, qint64 bytesTotal)
{
{
if
(
bytesTotal
)
{
if
(
bytesTotal
)
{
qreal
progress
=
(
qreal
)
bytesSent
/
(
qreal
)
bytesTotal
;
qreal
progress
=
(
qreal
)
bytesSent
/
(
qreal
)
bytesTotal
;
if
(
_currentLogfile
)
if
(
_currentLogfile
)
{
_currentLogfile
->
setProgress
(
progress
);
_currentLogfile
->
setProgress
(
progress
);
}
}
}
qCDebug
(
MavlinkLogManagerLog
)
<<
bytesSent
<<
"of"
<<
bytesTotal
;
qCDebug
(
MavlinkLogManagerLog
)
<<
bytesSent
<<
"of"
<<
bytesTotal
;
}
}
...
@@ -393,17 +446,15 @@ MavlinkLogManager::_activeVehicleChanged(Vehicle* vehicle)
...
@@ -393,17 +446,15 @@ MavlinkLogManager::_activeVehicleChanged(Vehicle* vehicle)
// connects/disconnects. In reality, if QGC is connected to multiple vehicles,
// 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
// 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.
// far, I'm working on the assumption that multiple vehicles is a rare exception.
// Disconnect the previous one (if any)
// Disconnect the previous one (if any)
if
(
_vehicle
)
{
if
(
_vehicle
)
{
disconnect
(
_vehicle
,
&
Vehicle
::
armedChanged
,
this
,
&
MavlinkLogManager
::
_armedChanged
);
disconnect
(
_vehicle
,
&
Vehicle
::
armedChanged
,
this
,
&
MavlinkLogManager
::
_armedChanged
);
disconnect
(
_vehicle
,
&
Vehicle
::
mavlinkLogData
,
this
,
&
MavlinkLogManager
::
_mavlinkLogData
);
disconnect
(
_vehicle
,
&
Vehicle
::
mavlinkLogData
,
this
,
&
MavlinkLogManager
::
_mavlinkLogData
);
_vehicle
=
NULL
;
_vehicle
=
NULL
;
emit
canStartLogChanged
();
emit
canStartLogChanged
();
}
}
// Connect new system
// Connect new system
if
(
vehicle
)
if
(
vehicle
)
{
{
_vehicle
=
vehicle
;
_vehicle
=
vehicle
;
connect
(
_vehicle
,
&
Vehicle
::
armedChanged
,
this
,
&
MavlinkLogManager
::
_armedChanged
);
connect
(
_vehicle
,
&
Vehicle
::
armedChanged
,
this
,
&
MavlinkLogManager
::
_armedChanged
);
connect
(
_vehicle
,
&
Vehicle
::
mavlinkLogData
,
this
,
&
MavlinkLogManager
::
_mavlinkLogData
);
connect
(
_vehicle
,
&
Vehicle
::
mavlinkLogData
,
this
,
&
MavlinkLogManager
::
_mavlinkLogData
);
...
@@ -413,10 +464,47 @@ MavlinkLogManager::_activeVehicleChanged(Vehicle* vehicle)
...
@@ -413,10 +464,47 @@ MavlinkLogManager::_activeVehicleChanged(Vehicle* vehicle)
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
void
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*/
)
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*/
)
{
if
(
_currentSavingFileFd
)
{
if
(
sequence
!=
_sequence
)
{
qCWarning
(
MavlinkLogManagerLog
)
<<
"Dropped Mavlink log data"
;
if
(
first_message
<
255
)
{
data
+=
first_message
;
length
-=
first_message
;
}
else
{
return
;
}
}
if
(
fwrite
(
data
,
1
,
length
,
_currentSavingFileFd
)
!=
(
size_t
)
length
)
{
fclose
(
_currentSavingFileFd
);
_currentSavingFileFd
=
NULL
;
qCCritical
(
MavlinkLogManagerLog
)
<<
"Error writing Mavlink log file:"
<<
_currentSavingFileStr
;
_logRunning
=
false
;
_vehicle
->
stopMavlinkLog
();
emit
logRunningChanged
();
}
}
else
{
qCWarning
(
MavlinkLogManagerLog
)
<<
"Mavlink log data received when not expected."
;
}
_sequence
=
sequence
+
1
;
}
//-----------------------------------------------------------------------------
bool
MavlinkLogManager
::
_createNewLog
()
{
{
Q_UNUSED
(
data
);
if
(
_currentSavingFileFd
)
{
qDebug
()
<<
"Mavlink Log:"
<<
sequence
<<
length
<<
first_message
;
fclose
(
_currentSavingFileFd
);
}
_currentSavingFileStr
.
sprintf
(
"%s/%03d-%s.ulg"
,
_logPath
.
toLatin1
().
data
(),
_vehicle
->
id
(),
QDateTime
::
currentDateTime
().
toString
(
"yyyy-MM-dd-hh-mm-ss-zzz"
).
toLatin1
().
data
());
_currentSavingFileFd
=
fopen
(
_currentSavingFileStr
.
toLatin1
().
data
(),
"wb"
);
if
(
!
_currentSavingFileFd
)
{
qCCritical
(
MavlinkLogManagerLog
)
<<
"Could not create Mavlink log file:"
<<
_currentSavingFileStr
;
_currentSavingFileStr
.
clear
();
}
_sequence
=
0
;
return
_currentSavingFileFd
!=
NULL
;
}
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
...
@@ -426,17 +514,11 @@ MavlinkLogManager::_armedChanged(bool armed)
...
@@ -426,17 +514,11 @@ MavlinkLogManager::_armedChanged(bool armed)
if
(
_vehicle
)
{
if
(
_vehicle
)
{
if
(
armed
)
{
if
(
armed
)
{
if
(
_enableAutoStart
)
{
if
(
_enableAutoStart
)
{
_vehicle
->
startMavlinkLog
();
startLogging
();
_logRunning
=
true
;
emit
logRunningChanged
();
}
}
}
else
{
}
else
{
if
(
_logRunning
&&
_enableAutoStart
)
{
if
(
_logRunning
&&
_enableAutoStart
)
{
_vehicle
->
stopMavlinkLog
();
stopLogging
();
emit
logRunningChanged
();
if
(
_enableAutoUpload
)
{
//-- TODO: Queue log for auto upload
}
}
}
}
}
}
}
...
...
src/uas/MavlinkLogManager.h
View file @
4d23912a
...
@@ -131,6 +131,8 @@ private slots:
...
@@ -131,6 +131,8 @@ private slots:
private:
private:
bool
_sendLog
(
const
QString
&
logFile
);
bool
_sendLog
(
const
QString
&
logFile
);
bool
_processUploadResponse
(
int
http_code
,
QByteArray
&
data
);
bool
_processUploadResponse
(
int
http_code
,
QByteArray
&
data
);
bool
_createNewLog
();
int
_getFirstSelected
();
private:
private:
QString
_description
;
QString
_description
;
...
@@ -144,6 +146,10 @@ private:
...
@@ -144,6 +146,10 @@ private:
MavlinkLogFiles
*
_currentLogfile
;
MavlinkLogFiles
*
_currentLogfile
;
Vehicle
*
_vehicle
;
Vehicle
*
_vehicle
;
bool
_logRunning
;
bool
_logRunning
;
bool
_loggingDisabled
;
FILE
*
_currentSavingFileFd
;
QString
_currentSavingFileStr
;
uint16_t
_sequence
;
};
};
#endif
#endif
src/ui/preferences/MavlinkSettings.qml
View file @
4d23912a
...
@@ -37,12 +37,10 @@ Rectangle {
...
@@ -37,12 +37,10 @@ Rectangle {
var
selected
=
0
var
selected
=
0
for
(
var
i
=
0
;
i
<
QGroundControl
.
mavlinkLogManager
.
logFiles
.
count
;
i
++
)
{
for
(
var
i
=
0
;
i
<
QGroundControl
.
mavlinkLogManager
.
logFiles
.
count
;
i
++
)
{
var
logFile
=
QGroundControl
.
mavlinkLogManager
.
logFiles
.
get
(
i
)
var
logFile
=
QGroundControl
.
mavlinkLogManager
.
logFiles
.
get
(
i
)
console
.
log
(
logFile
.
selected
)
if
(
logFile
.
selected
)
if
(
logFile
.
selected
)
selected
++
selected
++
}
}
_selectedCount
=
selected
_selectedCount
=
selected
console
.
log
(
_selectedCount
)
}
}
}
}
...
@@ -296,26 +294,27 @@ Rectangle {
...
@@ -296,26 +294,27 @@ Rectangle {
id
:
logFilesColumn
id
:
logFilesColumn
spacing
:
ScreenTools
.
defaultFontPixelWidth
spacing
:
ScreenTools
.
defaultFontPixelWidth
anchors.centerIn
:
parent
anchors.centerIn
:
parent
width
:
ScreenTools
.
defaultFontPixelWidth
*
68
Rectangle
{
Rectangle
{
width
:
ScreenTools
.
defaultFontPixelWidth
*
52
width
:
ScreenTools
.
defaultFontPixelWidth
*
64
height
:
ScreenTools
.
defaultFontPixelHeight
*
10
height
:
ScreenTools
.
defaultFontPixelHeight
*
10
anchors.horizontalCenter
:
parent
.
horizontalCenter
anchors.horizontalCenter
:
parent
.
horizontalCenter
color
:
qgcPal
.
window
Shade
color
:
qgcPal
.
window
border.color
:
qgcPal
.
text
border.color
:
qgcPal
.
text
border.width
:
0.5
border.width
:
0.5
ListView
{
ListView
{
width
:
ScreenTools
.
defaultFontPixelWidth
*
5
0
width
:
ScreenTools
.
defaultFontPixelWidth
*
5
6
height
:
ScreenTools
.
defaultFontPixelHeight
*
9
height
:
ScreenTools
.
defaultFontPixelHeight
*
8.75
anchors.centerIn
:
parent
anchors.centerIn
:
parent
orientation
:
ListView
.
Vertical
orientation
:
ListView
.
Vertical
model
:
QGroundControl
.
mavlinkLogManager
.
logFiles
model
:
QGroundControl
.
mavlinkLogManager
.
logFiles
clip
:
true
delegate
:
Rectangle
{
delegate
:
Rectangle
{
width
:
ScreenTools
.
defaultFontPixelWidth
*
48
width
:
ScreenTools
.
defaultFontPixelWidth
*
52
height
:
ScreenTools
.
defaultFontPixelHeight
*
1.25
height
:
ScreenTools
.
defaultFontPixelHeight
*
1.25
color
:
index
%
2
==
0
?
qgcPal
.
window
:
qgcPal
.
windowShade
color
:
qgcPal
.
window
anchors.horizontalCenter
:
parent
.
horizontalCenter
;
Row
{
Row
{
width
:
ScreenTools
.
defaultFontPixelWidth
*
46
width
:
ScreenTools
.
defaultFontPixelWidth
*
50
anchors.centerIn
:
parent
anchors.centerIn
:
parent
spacing
:
ScreenTools
.
defaultFontPixelWidth
spacing
:
ScreenTools
.
defaultFontPixelWidth
QGCCheckBox
{
QGCCheckBox
{
...
@@ -327,10 +326,10 @@ Rectangle {
...
@@ -327,10 +326,10 @@ Rectangle {
}
}
QGCLabel
{
QGCLabel
{
text
:
object
.
name
text
:
object
.
name
width
:
ScreenTools
.
defaultFontPixelWidth
*
2
0
width
:
ScreenTools
.
defaultFontPixelWidth
*
2
8
}
}
QGCLabel
{
QGCLabel
{
text
:
object
.
size
text
:
Number
(
object
.
size
).
toLocaleString
(
Qt
.
locale
(),
'
f
'
,
0
)
visible
:
!
object
.
uploading
visible
:
!
object
.
uploading
width
:
ScreenTools
.
defaultFontPixelWidth
*
20
;
width
:
ScreenTools
.
defaultFontPixelWidth
*
20
;
horizontalAlignment
:
Text
.
AlignRight
horizontalAlignment
:
Text
.
AlignRight
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment