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
a412dfef
Commit
a412dfef
authored
Feb 13, 2011
by
pixhawk
Browse files
Options
Browse Files
Download
Plain Diff
Merged
parents
49a778ed
a11ca3da
Changes
45
Expand all
Hide whitespace changes
Inline
Side-by-side
Showing
45 changed files
with
1219 additions
and
294 deletions
+1219
-294
style-mission.css
images/style-mission.css
+35
-0
mapnetwork.cpp
lib/QMapControl/src/mapnetwork.cpp
+9
-6
qgroundcontrol.pro
qgroundcontrol.pro
+6
-3
GAudioOutput.cc
src/GAudioOutput.cc
+8
-4
GAudioOutput.h
src/GAudioOutput.h
+6
-2
LinkManager.h
src/comm/LinkManager.h
+3
-0
MAVLinkProtocol.cc
src/comm/MAVLinkProtocol.cc
+62
-0
MAVLinkProtocol.h
src/comm/MAVLinkProtocol.h
+42
-0
MAVLinkSimulationLink.cc
src/comm/MAVLinkSimulationLink.cc
+70
-33
MAVLinkSimulationMAV.cc
src/comm/MAVLinkSimulationMAV.cc
+11
-9
MAVLinkSimulationWaypointPlanner.cc
src/comm/MAVLinkSimulationWaypointPlanner.cc
+2
-2
PxQuadMAV.cc
src/uas/PxQuadMAV.cc
+1
-1
UAS.cc
src/uas/UAS.cc
+145
-36
UAS.h
src/uas/UAS.h
+7
-1
UASInterface.h
src/uas/UASInterface.h
+6
-0
UASWaypointManager.cc
src/uas/UASWaypointManager.cc
+6
-4
DebugConsole.cc
src/ui/DebugConsole.cc
+28
-19
DebugConsole.h
src/ui/DebugConsole.h
+1
-0
DebugConsole.ui
src/ui/DebugConsole.ui
+7
-1
HSIDisplay.cc
src/ui/HSIDisplay.cc
+6
-4
HUD.cc
src/ui/HUD.cc
+1
-1
MAVLinkSettingsWidget.cc
src/ui/MAVLinkSettingsWidget.cc
+60
-11
MAVLinkSettingsWidget.h
src/ui/MAVLinkSettingsWidget.h
+1
-0
MAVLinkSettingsWidget.ui
src/ui/MAVLinkSettingsWidget.ui
+138
-1
MainWindow.cc
src/ui/MainWindow.cc
+25
-11
MainWindow.h
src/ui/MainWindow.h
+3
-0
MainWindow.ui
src/ui/MainWindow.ui
+2
-2
MapWidget.cc
src/ui/MapWidget.cc
+6
-4
ParameterInterface.cc
src/ui/ParameterInterface.cc
+0
-8
QGCParamWidget.cc
src/ui/QGCParamWidget.cc
+117
-2
QGCParamWidget.h
src/ui/QGCParamWidget.h
+16
-0
QGCSensorSettingsWidget.cc
src/ui/QGCSensorSettingsWidget.cc
+1
-0
QGCSettingsWidget.cc
src/ui/QGCSettingsWidget.cc
+46
-0
QGCSettingsWidget.h
src/ui/QGCSettingsWidget.h
+22
-0
QGCSettingsWidget.ui
src/ui/QGCSettingsWidget.ui
+91
-0
UASView.ui
src/ui/UASView.ui
+60
-65
WaypointList.cc
src/ui/WaypointList.cc
+107
-40
WaypointView.cc
src/ui/WaypointView.cc
+7
-12
LinechartWidget.cc
src/ui/linechart/LinechartWidget.cc
+15
-3
LinechartWidget.h
src/ui/linechart/LinechartWidget.h
+3
-0
MAV2DIcon.cc
src/ui/map/MAV2DIcon.cc
+10
-4
QGCGoogleEarthView.cc
src/ui/map3D/QGCGoogleEarthView.cc
+2
-2
QGCGoogleEarthView.h
src/ui/map3D/QGCGoogleEarthView.h
+1
-1
UASView.cc
src/ui/uas/UASView.cc
+20
-2
UASView.h
src/ui/uas/UASView.h
+4
-0
No files found.
images/style-mission.css
View file @
a412dfef
...
...
@@ -356,3 +356,38 @@ QProgressBar::chunk#speedBar {
QProgressBar
::chunk
#thrustBar
{
background-color
:
orange
;
}
QDialog
{
border
:
1px
solid
#62676B
;
border-radius
:
2px
;
}
QTabWidget
::pane
{
/* The tab widget frame */
border
:
1px
solid
#62676B
;
border-radius
:
2px
;
position
:
absolute
;
top
:
-0.5em
;
}
QTabWidget
::tab-bar
{
alignment
:
center
;
}
/* Style the tab using the tab sub-control. Note that
it reads QTabBar _not_ QTabWidget */
QTabBar
::tab
{
background-color
:
qlineargradient
(
x1
:
0
,
y1
:
0
,
x2
:
0
,
y2
:
1
,
stop
:
0
#090909
,
stop
:
1
#353535
);
border
:
2px
solid
#62676B
;
border-radius
:
4px
;
min-width
:
8ex
;
padding
:
2px
;
}
QTabBar
::tab:selected
,
QTabBar
::tab:hover
{
background-color
:
qlineargradient
(
x1
:
0
,
y1
:
0
,
x2
:
0
,
y2
:
1
,
stop
:
0
#090909
,
stop
:
1
#353535
);
border
:
2px
solid
#379AC3
;
}
QTabBar
::tab:selected
{
border
:
2px
solid
#379AC3
;
}
lib/QMapControl/src/mapnetwork.cpp
View file @
a412dfef
...
...
@@ -63,10 +63,10 @@ namespace qmapcontrol
void
MapNetwork
::
requestFinished
(
int
id
,
bool
error
)
{
// sleep(1);
// qDebug() << "
MapNetwork::requestFinished" << http->state() << ", id: " << id;
qDebug
()
<<
"QMapControl:
MapNetwork::requestFinished"
<<
http
->
state
()
<<
", id: "
<<
id
;
if
(
error
)
{
qDebug
()
<<
"network error: "
<<
http
->
errorString
();
qDebug
()
<<
"
QMapControl:
network error: "
<<
http
->
errorString
();
//restart query
}
...
...
@@ -79,7 +79,7 @@ namespace qmapcontrol
QString
url
=
loadingMap
[
id
];
loadingMap
.
remove
(
id
);
vectorMutex
.
unlock
();
//
qDebug() << "
request finished for id: " << id << ", belongs to: " << notifier.url << endl;
//
qDebug() << "QMapControl:
request finished for id: " << id << ", belongs to: " << notifier.url << endl;
QByteArray
ax
;
if
(
http
->
bytesAvailable
()
>
0
)
...
...
@@ -87,17 +87,20 @@ namespace qmapcontrol
QPixmap
pm
;
ax
=
http
->
readAll
();
qDebug
()
<<
"QMapControl: Request consisted of "
<<
ax
.
size
()
<<
"bytes"
;
if
(
pm
.
loadFromData
(
ax
))
{
loaded
+=
pm
.
size
().
width
()
*
pm
.
size
().
height
()
*
pm
.
depth
()
/
8
/
1024
;
// qDebug() << "
Network loaded: " << (loaded);
qDebug
()
<<
"QMapControl:
Network loaded: "
<<
(
loaded
);
parent
->
receivedImage
(
pm
,
url
);
}
else
if
(
pm
.
width
()
==
0
||
pm
.
height
()
==
0
)
{
// Silently ignore map request for a
// 0xn pixel map
qDebug
()
<<
"QMapControl: IGNORED 0x0 pixel map request, widthxheight:"
<<
pm
.
width
()
<<
"x"
<<
pm
.
height
();
qDebug
()
<<
"QMapControl: HTML ERROR MESSAGE:"
<<
ax
<<
"at "
<<
__FILE__
<<
__LINE__
;
}
else
{
...
...
@@ -105,7 +108,7 @@ namespace qmapcontrol
// TODO Error is currently undetected
//qDebug() << "NETWORK_PIXMAP_ERROR: " << ax;
qDebug
()
<<
"QMapControl external library: ERROR loading map:"
<<
"width:"
<<
pm
.
width
()
<<
"heigh:"
<<
pm
.
height
()
<<
"at "
<<
__FILE__
<<
__LINE__
;
//qDebug() << "
HTML ERROR MESSAGE:" << ax << "at " << __FILE__ << __LINE__;
qDebug
()
<<
"QMapControl:
HTML ERROR MESSAGE:"
<<
ax
<<
"at "
<<
__FILE__
<<
__LINE__
;
}
}
...
...
qgroundcontrol.pro
View file @
a412dfef
...
...
@@ -157,7 +157,8 @@ FORMS += src/ui/MainWindow.ui \
src
/
ui
/
QGCMAVLinkLogPlayer
.
ui
\
src
/
ui
/
QGCWaypointListMulti
.
ui
\
src
/
ui
/
mission
/
QGCCustomWaypointAction
.
ui
\
src
/
ui
/
QGCUDPLinkConfiguration
.
ui
src
/
ui
/
QGCUDPLinkConfiguration
.
ui
\
src
/
ui
/
QGCSettingsWidget
.
ui
INCLUDEPATH
+=
src
\
src
/
ui
\
...
...
@@ -268,7 +269,8 @@ HEADERS += src/MG.h \
src
/
comm
/
MAVLinkSimulationMAV
.
h
\
src
/
uas
/
QGCMAVLinkUASFactory
.
h
\
src
/
ui
/
QGCWaypointListMulti
.
h
\
src
/
ui
/
QGCUDPLinkConfiguration
.
h
src
/
ui
/
QGCUDPLinkConfiguration
.
h
\
src
/
ui
/
QGCSettingsWidget
.
h
#
Google
Earth
is
only
supported
on
Mac
OS
and
Windows
with
Visual
Studio
Compiler
macx
|
win32
-
msvc2008
:
{
...
...
@@ -394,7 +396,8 @@ SOURCES += src/main.cc \
src
/
comm
/
MAVLinkSimulationMAV
.
cc
\
src
/
uas
/
QGCMAVLinkUASFactory
.
cc
\
src
/
ui
/
QGCWaypointListMulti
.
cc
\
src
/
ui
/
QGCUDPLinkConfiguration
.
cc
src
/
ui
/
QGCUDPLinkConfiguration
.
cc
\
src
/
ui
/
QGCSettingsWidget
.
cc
macx
|
win32
-
msvc2008
:
{
SOURCES
+=
src
/
ui
/
map3D
/
QGCGoogleEarthView
.
cc
...
...
src/GAudioOutput.cc
View file @
a412dfef
...
...
@@ -151,10 +151,14 @@ GAudioOutput::~GAudioOutput()
void
GAudioOutput
::
mute
(
bool
mute
)
{
this
->
muted
=
mute
;
QSettings
settings
;
settings
.
setValue
(
QGC_GAUDIOOUTPUT_KEY
+
"muted"
,
this
->
muted
);
settings
.
sync
();
if
(
mute
!=
muted
)
{
this
->
muted
=
mute
;
QSettings
settings
;
settings
.
setValue
(
QGC_GAUDIOOUTPUT_KEY
+
"muted"
,
this
->
muted
);
settings
.
sync
();
emit
mutedChanged
(
muted
);
}
}
bool
GAudioOutput
::
isMuted
()
...
...
src/GAudioOutput.h
View file @
a412dfef
...
...
@@ -80,6 +80,9 @@ public:
VOICE_FEMALE
}
QGVoice
;
/** @brief Get the mute state */
bool
isMuted
();
public
slots
:
/** @brief Say this text if current output priority matches */
bool
say
(
QString
text
,
int
severity
=
1
);
...
...
@@ -101,8 +104,9 @@ public slots:
void
notifyNegative
();
/** @brief Mute/unmute sound */
void
mute
(
bool
mute
);
/** @brief Get the mute state */
bool
isMuted
();
signals:
void
mutedChanged
(
bool
);
protected:
#ifdef Q_OS_MAC
...
...
src/comm/LinkManager.h
View file @
a412dfef
...
...
@@ -62,6 +62,9 @@ public:
/** @brief Get a list of all links */
const
QList
<
LinkInterface
*>
getLinks
();
/** @brief Get a list of all protocols */
const
QList
<
ProtocolInterface
*>
getProtocols
()
{
return
protocolLinks
.
uniqueKeys
();
}
public
slots
:
void
add
(
LinkInterface
*
link
);
...
...
src/comm/MAVLinkProtocol.cc
View file @
a412dfef
...
...
@@ -44,6 +44,11 @@ MAVLinkProtocol::MAVLinkProtocol() :
m_loggingEnabled
(
false
),
m_logfile
(
NULL
),
m_enable_version_check
(
true
),
m_paramRetransmissionTimeout
(
350
),
m_paramRewriteTimeout
(
500
),
m_paramGuardEnabled
(
true
),
m_actionGuardEnabled
(
false
),
m_actionRetransmissionTimeout
(
100
),
versionMismatchIgnore
(
false
),
systemId
(
QGC
::
defaultSystemId
)
{
...
...
@@ -95,6 +100,14 @@ void MAVLinkProtocol::loadSettings()
{
systemId
=
temp
;
}
// Parameter interface settings
bool
ok
;
temp
=
settings
.
value
(
"PARAMETER_RETRANSMISSION_TIMEOUT"
,
m_paramRetransmissionTimeout
).
toInt
(
&
ok
);
if
(
ok
)
m_paramRetransmissionTimeout
=
temp
;
temp
=
settings
.
value
(
"PARAMETER_REWRITE_TIMEOUT"
,
m_paramRewriteTimeout
).
toInt
(
&
ok
);
if
(
ok
)
m_paramRewriteTimeout
=
temp
;
m_paramGuardEnabled
=
settings
.
value
(
"PARAMETER_TRANSMISSION_GUARD_ENABLED"
,
m_paramGuardEnabled
).
toBool
();
settings
.
endGroup
();
}
...
...
@@ -113,6 +126,10 @@ void MAVLinkProtocol::storeSettings()
// Logfile exists, store the name
settings
.
setValue
(
"LOGFILE_NAME"
,
m_logfile
->
fileName
());
}
// Parameter interface settings
settings
.
setValue
(
"PARAMETER_RETRANSMISSION_TIMEOUT"
,
m_paramRetransmissionTimeout
);
settings
.
setValue
(
"PARAMETER_REWRITE_TIMEOUT"
,
m_paramRewriteTimeout
);
settings
.
setValue
(
"PARAMETER_TRANSMISSION_GUARD_ENABLED"
,
m_paramGuardEnabled
);
settings
.
endGroup
();
settings
.
sync
();
//qDebug() << "Storing settings!";
...
...
@@ -418,6 +435,51 @@ void MAVLinkProtocol::enableMultiplexing(bool enabled)
if
(
changed
)
emit
multiplexingChanged
(
m_multiplexingEnabled
);
}
void
MAVLinkProtocol
::
enableParamGuard
(
bool
enabled
)
{
if
(
enabled
!=
m_paramGuardEnabled
)
{
m_paramGuardEnabled
=
enabled
;
emit
paramGuardChanged
(
m_paramGuardEnabled
);
}
}
void
MAVLinkProtocol
::
enableActionGuard
(
bool
enabled
)
{
if
(
enabled
!=
m_actionGuardEnabled
)
{
m_actionGuardEnabled
=
enabled
;
emit
actionGuardChanged
(
m_actionGuardEnabled
);
}
}
void
MAVLinkProtocol
::
setParamRetransmissionTimeout
(
int
ms
)
{
if
(
ms
!=
m_paramRetransmissionTimeout
)
{
m_paramRetransmissionTimeout
=
ms
;
emit
paramRetransmissionTimeoutChanged
(
m_paramRetransmissionTimeout
);
}
}
void
MAVLinkProtocol
::
setParamRewriteTimeout
(
int
ms
)
{
if
(
ms
!=
m_paramRewriteTimeout
)
{
m_paramRewriteTimeout
=
ms
;
emit
paramRewriteTimeoutChanged
(
m_paramRewriteTimeout
);
}
}
void
MAVLinkProtocol
::
setActionRetransmissionTimeout
(
int
ms
)
{
if
(
ms
!=
m_actionRetransmissionTimeout
)
{
m_actionRetransmissionTimeout
=
ms
;
emit
actionRetransmissionTimeoutChanged
(
m_actionRetransmissionTimeout
);
}
}
void
MAVLinkProtocol
::
enableLogging
(
bool
enabled
)
{
bool
changed
=
false
;
...
...
src/comm/MAVLinkProtocol.h
View file @
a412dfef
...
...
@@ -77,6 +77,16 @@ public:
int
getVersion
()
{
return
MAVLINK_VERSION
;
}
/** @brief Get the name of the packet log file */
QString
getLogfileName
();
/** @brief Get state of parameter retransmission */
bool
paramGuardEnabled
()
{
return
m_paramGuardEnabled
;
}
/** @brief Get parameter read timeout */
int
getParamRetransmissionTimeout
()
{
return
m_paramRetransmissionTimeout
;
}
/** @brief Get parameter write timeout */
int
getParamRewriteTimeout
()
{
return
m_paramRewriteTimeout
;
}
/** @brief Get state of action retransmission */
bool
actionGuardEnabled
()
{
return
m_actionGuardEnabled
;
}
/** @brief Get parameter read timeout */
int
getActionRetransmissionTimeout
()
{
return
m_actionRetransmissionTimeout
;
}
public
slots
:
/** @brief Receive bytes from a communication interface */
...
...
@@ -99,6 +109,21 @@ public slots:
/** @brief Enabled/disable packet multiplexing */
void
enableMultiplexing
(
bool
enabled
);
/** @brief Enable / disable parameter retransmission */
void
enableParamGuard
(
bool
enabled
);
/** @brief Enable / disable action retransmission */
void
enableActionGuard
(
bool
enabled
);
/** @brief Set parameter read timeout */
void
setParamRetransmissionTimeout
(
int
ms
);
/** @brief Set parameter write timeout */
void
setParamRewriteTimeout
(
int
ms
);
/** @brief Set parameter read timeout */
void
setActionRetransmissionTimeout
(
int
ms
);
/** @brief Set log file name */
void
setLogfileName
(
const
QString
&
filename
);
...
...
@@ -121,6 +146,11 @@ protected:
bool
m_multiplexingEnabled
;
///< Enable/disable packet multiplexing
QFile
*
m_logfile
;
///< Logfile
bool
m_enable_version_check
;
///< Enable checking of version match of MAV and QGC
int
m_paramRetransmissionTimeout
;
///< Timeout for parameter retransmission
int
m_paramRewriteTimeout
;
///< Timeout for sending re-write request
bool
m_paramGuardEnabled
;
///< Parameter retransmission/rewrite enabled
bool
m_actionGuardEnabled
;
///< Action request retransmission enabled
int
m_actionRetransmissionTimeout
;
///< Timeout for parameter retransmission
QMutex
receiveMutex
;
///< Mutex to protect receiveBytes function
int
lastIndex
[
256
][
256
];
int
totalReceiveCounter
;
...
...
@@ -143,6 +173,18 @@ signals:
void
versionCheckChanged
(
bool
enabled
);
/** @brief Emitted if a message from the protocol should reach the user */
void
protocolStatusMessage
(
const
QString
&
title
,
const
QString
&
message
);
/** @brief Emitted if a new system ID was set */
void
systemIdChanged
(
int
systemId
);
/** @brief Emitted if param guard status changed */
void
paramGuardChanged
(
bool
enabled
);
/** @brief Emitted if param read timeout changed */
void
paramRetransmissionTimeoutChanged
(
int
ms
);
/** @brief Emitted if param write timeout changed */
void
paramRewriteTimeoutChanged
(
int
ms
);
/** @brief Emitted if action guard status changed */
void
actionGuardChanged
(
bool
enabled
);
/** @brief Emitted if actiion request timeout changed */
void
actionRetransmissionTimeoutChanged
(
int
ms
);
};
#endif // MAVLINKPROTOCOL_H_
src/comm/MAVLinkSimulationLink.cc
View file @
a412dfef
...
...
@@ -817,8 +817,8 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
qDebug
()
<<
"GCS REQUESTED PARAM LIST FROM SIMULATION"
;
mavlink_param_request_list_t
read
;
mavlink_msg_param_request_list_decode
(
&
msg
,
&
read
);
if
(
read
.
target_system
==
systemId
)
{
//
if (read.target_system == systemId)
//
{
// Output all params
// Iterate through all components, through all parameters and emit them
QMap
<
QString
,
float
>::
iterator
i
;
...
...
@@ -826,18 +826,21 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
int
j
=
0
;
for
(
i
=
onboardParams
.
begin
();
i
!=
onboardParams
.
end
();
++
i
)
{
// Pack message and get size of encoded byte string
mavlink_msg_param_value_pack
(
systemId
,
componentId
,
&
msg
,
(
int8_t
*
)
i
.
key
().
toStdString
().
c_str
(),
i
.
value
(),
onboardParams
.
size
(),
j
);
// Allocate buffer with packet data
bufferlength
=
mavlink_msg_to_send_buffer
(
buffer
,
&
msg
);
//add data into datastream
memcpy
(
stream
+
streampointer
,
buffer
,
bufferlength
);
streampointer
+=
bufferlength
;
if
(
j
!=
5
)
{
// Pack message and get size of encoded byte string
mavlink_msg_param_value_pack
(
read
.
target_system
,
componentId
,
&
msg
,
(
int8_t
*
)
i
.
key
().
toStdString
().
c_str
(),
i
.
value
(),
onboardParams
.
size
(),
j
);
// Allocate buffer with packet data
bufferlength
=
mavlink_msg_to_send_buffer
(
buffer
,
&
msg
);
//add data into datastream
memcpy
(
stream
+
streampointer
,
buffer
,
bufferlength
);
streampointer
+=
bufferlength
;
}
j
++
;
}
qDebug
()
<<
"SIMULATION SENT PARAMETERS TO GCS"
;
}
//
}
}
break
;
case
MAVLINK_MSG_ID_PARAM_SET
:
...
...
@@ -845,8 +848,8 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
qDebug
()
<<
"SIMULATION RECEIVED COMMAND TO SET PARAMETER"
;
mavlink_param_set_t
set
;
mavlink_msg_param_set_decode
(
&
msg
,
&
set
);
if
(
set
.
target_system
==
systemId
)
{
//
if (set.target_system == systemId)
//
{
QString
key
=
QString
((
char
*
)
set
.
param_id
);
if
(
onboardParams
.
contains
(
key
))
{
...
...
@@ -854,13 +857,46 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
onboardParams
.
insert
(
key
,
set
.
param_value
);
// Pack message and get size of encoded byte string
mavlink_msg_param_value_pack
(
s
ystemId
,
componentId
,
&
msg
,
(
int8_t
*
)
key
.
toStdString
().
c_str
(),
set
.
param_value
,
onboardParams
.
size
(),
0
);
mavlink_msg_param_value_pack
(
s
et
.
target_system
,
componentId
,
&
msg
,
(
int8_t
*
)
key
.
toStdString
().
c_str
(),
set
.
param_value
,
onboardParams
.
size
(),
onboardParams
.
keys
().
indexOf
(
key
)
);
// Allocate buffer with packet data
bufferlength
=
mavlink_msg_to_send_buffer
(
buffer
,
&
msg
);
//add data into datastream
memcpy
(
stream
+
streampointer
,
buffer
,
bufferlength
);
streampointer
+=
bufferlength
;
}
// }
}
case
MAVLINK_MSG_ID_PARAM_REQUEST_READ
:
{
qDebug
()
<<
"SIMULATION RECEIVED COMMAND TO SEND PARAMETER"
;
mavlink_param_request_read_t
read
;
mavlink_msg_param_request_read_decode
(
&
msg
,
&
read
);
QByteArray
bytes
((
char
*
)
read
.
param_id
,
MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN
);
QString
key
=
QString
(
bytes
);
if
(
onboardParams
.
contains
(
key
))
{
float
paramValue
=
onboardParams
.
value
(
key
);
// Pack message and get size of encoded byte string
mavlink_msg_param_value_pack
(
read
.
target_system
,
componentId
,
&
msg
,
(
int8_t
*
)
key
.
toStdString
().
c_str
(),
paramValue
,
onboardParams
.
size
(),
onboardParams
.
keys
().
indexOf
(
key
));
// Allocate buffer with packet data
bufferlength
=
mavlink_msg_to_send_buffer
(
buffer
,
&
msg
);
//add data into datastream
memcpy
(
stream
+
streampointer
,
buffer
,
bufferlength
);
streampointer
+=
bufferlength
;
}
else
if
(
read
.
param_index
<
onboardParams
.
size
())
{
key
=
onboardParams
.
keys
().
at
(
read
.
param_index
);
float
paramValue
=
onboardParams
.
value
(
key
);
// Pack message and get size of encoded byte string
mavlink_msg_param_value_pack
(
systemId
,
componentId
,
&
msg
,
(
int8_t
*
)
key
.
toStdString
().
c_str
(),
paramValue
,
onboardParams
.
size
(),
onboardParams
.
keys
().
indexOf
(
key
));
// Allocate buffer with packet data
bufferlength
=
mavlink_msg_to_send_buffer
(
buffer
,
&
msg
);
//add data into datastream
memcpy
(
stream
+
streampointer
,
buffer
,
bufferlength
);
streampointer
+=
bufferlength
;
}
}
break
;
...
...
@@ -868,10 +904,10 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
}
//
unsigned char v=data[i];
//
fprintf(stderr,"%02x ", v);
unsigned
char
v
=
data
[
i
];
fprintf
(
stderr
,
"%02x "
,
v
);
}
//
fprintf(stderr,"\n");
fprintf
(
stderr
,
"
\n
"
);
readyBufferMutex
.
lock
();
for
(
int
i
=
0
;
i
<
streampointer
;
i
++
)
...
...
@@ -903,22 +939,22 @@ void MAVLinkSimulationLink::readBytes() {
readyBufferMutex
.
unlock
();
// if (len > 0)
// {
// qDebug() << "Simulation sent " << len << " bytes to groundstation: ";
//
// /* Increase write counter */
// //bitsSentTotal += size * 8;
//
// //Output all bytes as hex digits
// int i;
// for (i=0; i<len; i++)
// {
// unsigned int v=data[i];
// fprintf(stderr,"%02x ", v);
// }
// fprintf(stderr,"\n");
// }
// if (len > 0)
// {
// qDebug() << "Simulation sent " << len << " bytes to groundstation: ";
// /* Increase write counter */
// //bitsSentTotal += size * 8;
// //Output all bytes as hex digits
// int i;
// for (i=0; i<len; i++)
// {
// unsigned int v=data[i];
// fprintf(stderr,"%02x ", v);
// }
// fprintf(stderr,"\n");
// }
}
/**
...
...
@@ -957,8 +993,9 @@ bool MAVLinkSimulationLink::connect()
start
(
LowPriority
);
MAVLinkSimulationMAV
*
mav1
=
new
MAVLinkSimulationMAV
(
this
,
1
,
47.376
,
8.548
);
//MAVLinkSimulationMAV* mav2 = new MAVLinkSimulationMAV(this, 2);
Q_UNUSED
(
mav1
);
//MAVLinkSimulationMAV* mav2 = new MAVLinkSimulationMAV(this, 2);
//
//Q_UNUSED(mav2);
// timer->start(rate);
return
true
;
...
...
src/comm/MAVLinkSimulationMAV.cc
View file @
a412dfef
...
...
@@ -96,7 +96,7 @@ void MAVLinkSimulationMAV::mainloop()
{
//float trueyaw = atan2f(xm, ym);
float
newYaw
=
atan2f
(
xm
,
y
m
);
float
newYaw
=
atan2f
(
ym
,
x
m
);
if
(
fabs
(
yaw
-
newYaw
)
<
90
)
{
...
...
@@ -112,8 +112,8 @@ void MAVLinkSimulationMAV::mainloop()
//if (sqrt(xm*xm+ym*ym) > 0.0000001)
if
(
flying
)
{
x
+=
sin
(
yaw
)
*
radPer100ms
;
y
+=
cos
(
yaw
)
*
radPer100ms
;
x
+=
cos
(
yaw
)
*
radPer100ms
;
y
+=
sin
(
yaw
)
*
radPer100ms
;
z
+=
altPer100ms
*
zsign
;
}
...
...
@@ -135,7 +135,7 @@ void MAVLinkSimulationMAV::mainloop()
pos
.
alt
=
z
*
1000.0
;
pos
.
lat
=
x
*
1E7
;
pos
.
lon
=
y
*
1E7
;
pos
.
vx
=
10.0
f
*
100.0
f
;
pos
.
vx
=
sin
(
yaw
)
*
10.0
f
*
100.0
f
;
pos
.
vy
=
0
;
pos
.
vz
=
altPer100ms
*
10.0
f
*
100.0
f
*
zsign
*-
1.0
f
;
mavlink_msg_global_position_int_encode
(
systemid
,
MAV_COMP_ID_IMU
,
&
msg
,
&
pos
);
...
...
@@ -147,7 +147,7 @@ void MAVLinkSimulationMAV::mainloop()
attitude
.
usec
=
0
;
attitude
.
roll
=
0.0
f
;
attitude
.
pitch
=
0.0
f
;
attitude
.
yaw
=
yaw
-
(
M_PI
/
2.0
)
;
attitude
.
yaw
=
yaw
;
attitude
.
rollspeed
=
0.0
f
;
attitude
.
pitchspeed
=
0.0
f
;
attitude
.
yawspeed
=
0.0
f
;
...
...
@@ -170,11 +170,11 @@ void MAVLinkSimulationMAV::mainloop()
// VFR HUD
mavlink_vfr_hud_t
hud
;
hud
.
airspeed
=
pos
.
vx
;
hud
.
groundspeed
=
pos
.
vx
;
hud
.
alt
=
pos
.
alt
;
hud
.
airspeed
=
pos
.
vx
/
100.0
f
;
hud
.
groundspeed
=
pos
.
vx
/
100.0
f
;
hud
.
alt
=
z
;
hud
.
heading
=
static_cast
<
int
>
((
yaw
/
M_PI
)
*
180.0
f
+
180.0
f
)
%
360
;
hud
.
climb
=
pos
.
vz
;
hud
.
climb
=
pos
.
vz
/
100.0
f
;
hud
.
throttle
=
90
;
mavlink_msg_vfr_hud_encode
(
systemid
,
MAV_COMP_ID_IMU
,
&
msg
,
&
hud
);
link
->
sendMAVLinkMessage
(
&
msg
);
...
...
@@ -323,6 +323,7 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
{
case
MAV_ACTION_TAKEOFF
:
flying
=
true
;
nav_mode
=
MAV_NAV_LIFTOFF
;
ack
.
result
=
1
;
break
;
default:
...
...
@@ -345,6 +346,7 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
mavlink_msg_local_position_setpoint_set_decode
(
&
msg
,
&
sp
);
if
(
sp
.
target_system
==
this
->
systemid
)
{
nav_mode
=
MAV_NAV_WAYPOINT
;
previousSPX
=
nextSPX
;
previousSPY
=
nextSPY
;
previousSPZ
=
nextSPZ
;
...
...
src/comm/MAVLinkSimulationWaypointPlanner.cc
View file @
a412dfef
...
...
@@ -575,9 +575,9 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
float
x
=
static_cast
<
double
>
(
pos
.
lat
)
/
1E7
;
float
y
=
static_cast
<
double
>
(
pos
.
lon
)
/
1E7
;
float
z
=
static_cast
<
double
>
(
pos
.
alt
)
/
1000
;
//
float z = static_cast<double>(pos.alt)/1000;
qDebug
()
<<
"Received new position: x:"
<<
x
<<
"| y:"
<<
y
<<
"| z:"
<<
z
;
//
qDebug() << "Received new position: x:" << x << "| y:" << y << "| z:" << z;
posReached
=
false
;
yawReached
=
true
;
...
...
src/uas/PxQuadMAV.cc
View file @
a412dfef
...
...
@@ -143,7 +143,7 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit
errCountChanged
(
uasId
,
"IMU"
,
"SPI0"
,
status
.
spi0_err_count
);
emit
errCountChanged
(
uasId
,
"IMU"
,
"SPI1"
,
status
.
spi1_err_count
);
emit
errCountChanged
(
uasId
,
"IMU"
,
"UART"
,
status
.
uart_total_err_count
);
emit
valueChanged
(
uasId
,
"Load"
,
"%"
,
((
float
)
status
.
load
)
/
10
00.0
f
,
MG
::
TIME
::
getGroundTimeNow
());
emit
valueChanged
(
uasId
,
"Load"
,
"%"
,
((
float
)
status
.
load
)
/
10
.0
f
,
getUnixTime
());
}
break
;
default:
...
...
src/uas/UAS.cc
View file @
a412dfef
This diff is collapsed.
Click to expand it.
src/uas/UAS.h
View file @
a412dfef
...
...
@@ -130,8 +130,9 @@ protected: //COMMENTS FOR TEST UNIT
bool
batteryRemainingEstimateEnabled
;
///< If the estimate is enabled, QGC will try to estimate the remaining battery life
float
chargeLevel
;
///< Charge level of battery, in percent
int
timeRemaining
;
///< Remaining time calculated based on previous and current
unsigned
int
mode
;
///< The current mode of the MAV
int
mode
;
///< The current mode of the MAV
int
status
;
///< The current status of the MAV
int
navMode
;
///< The current navigation mode of the MAV
quint64
onboardTimeOffset
;
bool
controlRollManual
;
///< status flag, true if roll is controlled manually
...
...
@@ -173,6 +174,8 @@ public:
float
getChargeLevel
();
/** @brief Get the human-readable status message for this code */
void
getStatusForCode
(
int
statusCode
,
QString
&
uasState
,
QString
&
stateDescription
);
/** @brief Get the human-readable navigation mode translation for this mode */
QString
getNavModeText
(
int
mode
);
/** @brief Check if vehicle is in autonomous mode */
bool
isAuto
();
...
...
@@ -259,6 +262,9 @@ public slots:
/** @brief Request all parameters */
void
requestParameters
();
/** @brief Request a single parameter by index */
void
requestParameter
(
int
component
,
int
parameter
);
/** @brief Set a system parameter */
void
setParameter
(
const
int
component
,
const
QString
&
id
,
const
float
value
);
...
...
src/uas/UASInterface.h
View file @
a412dfef
...
...
@@ -226,6 +226,8 @@ public slots:
virtual
void
setLocalOriginAtCurrentGPSPosition
()
=
0
;
/** @brief Request all onboard parameters of all components */
virtual
void
requestParameters
()
=
0
;
/** @brief Request one specific onboard parameter */
virtual
void
requestParameter
(
int
component
,
int
parameter
)
=
0
;
/** @brief Write parameter to permanent storage */
virtual
void
writeParametersToStorage
()
=
0
;
/** @brief Read parameter from permanent storage */
...
...
@@ -304,7 +306,11 @@ signals:
void
poiFound
(
UASInterface
*
uas
,
int
type
,
int
colorIndex
,
QString
message
,
float
x
,
float
y
,
float
z
);
void
poiConnectionFound
(
UASInterface
*
uas
,
int
type
,
int
colorIndex
,
QString
message
,
float
x1
,
float
y1
,
float
z1
,
float
x2
,
float
y2
,
float
z2
);
/** @brief A text message from the system has been received */
void
textMessageReceived
(
int
uasid
,
int
componentid
,
int
severity
,
QString
text
);
void
navModeChanged
(
int
uasid
,
int
mode
,
const
QString
&
text
);
/**
* @brief Update the error count of a device
*
...
...
src/uas/UASWaypointManager.cc
View file @
a412dfef
...
...
@@ -236,6 +236,7 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m
{
if
(
systemId
==
uas
.
getUASID
()
&&
compId
==
MAV_COMP_ID_WAYPOINTPLANNER
)
{
// FIXME Petri
if
(
current_state
==
WP_SETCURRENT
)
{
protocol_timer
.
stop
();
...
...
@@ -257,11 +258,12 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m
}
}
emit
updateStatusString
(
QString
(
"New current waypoint %1"
).
arg
(
wpc
->
seq
));
//emit update to UI widgets
emit
currentWaypointChanged
(
wpc
->
seq
);
//qDebug() << "Updated waypoints list";
}
qDebug
()
<<
"new current waypoint"
<<
wpc
->
seq
;
emit
updateStatusString
(
QString
(
"New current waypoint %1"
).
arg
(
wpc
->
seq
));
//emit update to UI widgets
emit
currentWaypointChanged
(
wpc
->
seq
);
//qDebug() << "new current waypoint" << wpc->seq;
}
}
...
...
src/ui/DebugConsole.cc
View file @
a412dfef
...
...
@@ -67,7 +67,7 @@ DebugConsole::DebugConsole(QWidget *parent) :
// Hide sent text field - it is only useful after send has been hit
m_ui
->
sentText
->
setVisible
(
false
);
// Hide auto-send checkbox
m_ui
->
specialCheckBox
->
setVisible
(
false
);
//
m_ui->specialCheckBox->setVisible(false);
// Make text area not editable
m_ui
->
receiveText
->
setReadOnly
(
true
);
// Limit to 500 lines
...
...
@@ -85,10 +85,10 @@ DebugConsole::DebugConsole(QWidget *parent) :
snapShotTimer
.
setInterval
(
snapShotInterval
);
snapShotTimer
.
start
();
// Set hex checkbox checked
m_ui
->
hexCheckBox
->
setChecked
(
!
convertToAscii
);
m_ui
->
mavlinkCheckBox
->
setChecked
(
filterMAVLINK
);
m_ui
->
holdCheckBox
->
setChecked
(
autoHold
);
//
// Set hex checkbox checked
//
m_ui->hexCheckBox->setChecked(!convertToAscii);
//
m_ui->mavlinkCheckBox->setChecked(filterMAVLINK);
//
m_ui->holdCheckBox->setChecked(autoHold);
// Get a list of all existing links
links
=
QList
<
LinkInterface
*>
();
...
...
@@ -116,12 +116,10 @@ DebugConsole::DebugConsole(QWidget *parent) :
// Connect Checkbox
connect
(
m_ui
->
specialComboBox
,
SIGNAL
(
highlighted
(
QString
)),
this
,
SLOT
(
specialSymbolSelected
(
QString
)));
// Set add button invisible if auto add checkbox is checked
connect
(
m_ui
->
specialCheckBox
,
SIGNAL
(
clicked
(
bool
)),
m_ui
->
addSymbolButton
,
SLOT
(
setHidden
(
bool
)));
//
connect(m_ui->specialCheckBox, SIGNAL(clicked(bool)), m_ui->addSymbolButton, SLOT(setHidden(bool)));
// Allow to send via return
connect
(
m_ui
->
sendText
,
SIGNAL
(
returnPressed
()),
this
,
SLOT
(
sendBytes
()));
hold
(
false
);
loadSettings
();
// Warn user about not activated hold
...
...
@@ -131,6 +129,12 @@ DebugConsole::DebugConsole(QWidget *parent) :
}
}
void
DebugConsole
::
hideEvent
(
QHideEvent
*
event
)
{
Q_UNUSED
(
event
);
storeSettings
();
}
DebugConsole
::~
DebugConsole
()