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
f3cd7d6c
Commit
f3cd7d6c
authored
Nov 11, 2011
by
LM
Browse files
Options
Browse Files
Download
Plain Diff
Merge branch 'dev_senseSoarMavlinkv10' of
https://github.com/oberion/qgroundcontrol
into v10release
parents
94f9e534
d4616625
Changes
45
Expand all
Hide whitespace changes
Inline
Side-by-side
Showing
45 changed files
with
3463 additions
and
105 deletions
+3463
-105
.gitignore
.gitignore
+4
-0
earth.html
images/earth.html
+7
-7
qgroundcontrol.pro
qgroundcontrol.pro
+47
-23
QGCCore.cc
src/QGCCore.cc
+1
-0
LinkInterface.h
src/comm/LinkInterface.h
+1
-1
SerialLink.cc
src/comm/SerialLink.cc
+41
-13
SerialLink.h
src/comm/SerialLink.h
+4
-0
UDPLink.cc
src/comm/UDPLink.cc
+55
-17
UDPLink.h
src/comm/UDPLink.h
+5
-2
XbeeLink.cpp
src/comm/XbeeLink.cpp
+18
-2
XbeeLink.h
src/comm/XbeeLink.h
+4
-0
XbeeLinkInterface.h
src/comm/XbeeLinkInterface.h
+6
-0
QGCMAVLinkUASFactory.cc
src/uas/QGCMAVLinkUASFactory.cc
+10
-0
QGCMAVLinkUASFactory.h
src/uas/QGCMAVLinkUASFactory.h
+1
-0
UAS.h
src/uas/UAS.h
+3
-3
senseSoarMAV.cpp
src/uas/senseSoarMAV.cpp
+214
-0
senseSoarMAV.h
src/uas/senseSoarMAV.h
+29
-0
CommConfigurationWindow.cc
src/ui/CommConfigurationWindow.cc
+16
-12
DebugConsole.cc
src/ui/DebugConsole.cc
+7
-2
MainWindow.cc
src/ui/MainWindow.cc
+118
-12
MainWindow.h
src/ui/MainWindow.h
+1
-0
QGCParamWidget.h
src/ui/QGCParamWidget.h
+2
-2
XbeeConfigurationWindow.cpp
src/ui/XbeeConfigurationWindow.cpp
+74
-9
XbeeConfigurationWindow.h
src/ui/XbeeConfigurationWindow.h
+13
-0
libxbee.dll
thirdParty/libxbee/lib/libxbee.dll
+0
-0
libxbee.exp
thirdParty/libxbee/lib/libxbee.exp
+0
-0
libxbee.lib
thirdParty/libxbee/lib/libxbee.lib
+0
-0
libxbee.map
thirdParty/libxbee/lib/libxbee.map
+897
-0
SenseSoar.h
thirdParty/mavlink/include/SenseSoar/SenseSoar.h
+69
-0
mavlink.h
thirdParty/mavlink/include/SenseSoar/mavlink.h
+11
-0
mavlink_msg_cmd_airspeed_ack.h
.../mavlink/include/SenseSoar/mavlink_msg_cmd_airspeed_ack.h
+123
-0
mavlink_msg_cmd_airspeed_chng.h
...mavlink/include/SenseSoar/mavlink_msg_cmd_airspeed_chng.h
+123
-0
mavlink_msg_filt_rot_vel.h
...arty/mavlink/include/SenseSoar/mavlink_msg_filt_rot_vel.h
+104
-0
mavlink_msg_llc_out.h
thirdParty/mavlink/include/SenseSoar/mavlink_msg_llc_out.h
+124
-0
mavlink_msg_obs_air_temp.h
...arty/mavlink/include/SenseSoar/mavlink_msg_obs_air_temp.h
+106
-0
mavlink_msg_obs_air_velocity.h
.../mavlink/include/SenseSoar/mavlink_msg_obs_air_velocity.h
+150
-0
mavlink_msg_obs_attitude.h
...arty/mavlink/include/SenseSoar/mavlink_msg_obs_attitude.h
+104
-0
mavlink_msg_obs_bias.h
thirdParty/mavlink/include/SenseSoar/mavlink_msg_obs_bias.h
+124
-0
mavlink_msg_obs_position.h
...arty/mavlink/include/SenseSoar/mavlink_msg_obs_position.h
+150
-0
mavlink_msg_obs_qff.h
thirdParty/mavlink/include/SenseSoar/mavlink_msg_obs_qff.h
+106
-0
mavlink_msg_obs_velocity.h
...arty/mavlink/include/SenseSoar/mavlink_msg_obs_velocity.h
+104
-0
mavlink_msg_obs_wind.h
thirdParty/mavlink/include/SenseSoar/mavlink_msg_obs_wind.h
+104
-0
mavlink_msg_pm_elec.h
thirdParty/mavlink/include/SenseSoar/mavlink_msg_pm_elec.h
+148
-0
mavlink_msg_sys_stat.h
thirdParty/mavlink/include/SenseSoar/mavlink_msg_sys_stat.h
+152
-0
SenseSoar.xml
thirdParty/mavlink/message_definitions/SenseSoar.xml
+83
-0
No files found.
.gitignore
View file @
f3cd7d6c
...
...
@@ -34,6 +34,10 @@ user_config.pri
*.ncb
*.vcproj
*.vcxproj*
*.sdf
*.ipch
*.pdb
*.sln
*.sln
*.vcproj
...
...
images/earth.html
View file @
f3cd7d6c
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html>
<head>
...
...
@@ -415,9 +415,9 @@ function createAircraft(id, type, color)
planeModel
.
setLocation
(
planeLoc
);
planeLink
=
ge
.
createLink
(
''
);
planeOrient
=
ge
.
createOrientation
(
''
);
planeModel
.
setOrientation
(
planeOrient
);
planeModel
.
setOrientation
(
planeOrient
);
planeLink
.
setHref
(
'
http://
qgroundcontrol.org/_media/users/models/ascent-park-glider.dae
'
);
planeLink
.
setHref
(
'
http://
www.asl.ethz.ch/people/rudink/senseSoarDummy.dae
'
);
planeModel
.
setLink
(
planeLink
);
planeModel
.
setAltitudeMode
(
ge
.
ALTITUDE_ABSOLUTE
);
...
...
@@ -584,10 +584,10 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
//currFollowHeading = ((yaw/M_PI)+1.0)*360.0;
// FIXME Currently invalid conversion from right-handed z-down to z-up frame
planeOrient
.
set
Roll
(((
roll
/
M_PI
))
*
180.0
+
180.0
);
planeOrient
.
set
Tilt
(((
pitch
/
M_PI
))
*
180.0
+
180.0
);
plane
Orient
.
setHeading
(((
yaw
/
M_PI
))
*
180.0
-
90.0
);
planeOrient
.
setRoll
(
+
((
pitch
/
M_PI
))
*
180.0
);
planeOrient
.
set
Tilt
(
-
((
roll
/
M_PI
))
*
180.0
);
planeOrient
.
set
Heading
(((
yaw
/
M_PI
))
*
180.0
-
90.0
);
plane
Model
.
setOrientation
(
planeOrient
);
currFollowHeading
=
((
yaw
/
M_PI
))
*
180.0
;
...
...
qgroundcontrol.pro
View file @
f3cd7d6c
...
...
@@ -19,6 +19,8 @@
#
Qt
configuration
CONFIG
+=
qt
\
thread
QT
+=
network
\
opengl
\
svg
\
...
...
@@ -29,17 +31,25 @@ QT += network \
TEMPLATE
=
app
TARGET
=
qgroundcontrol
BASEDIR
=
$$
IN_PWD
TARGETDIR
=
$$
OUT_PWD
BUILDDIR
=
$$
TARGETDIR
/
build
BASEDIR
=
$$
{
IN_PWD
}
TARGETDIR
=
$$
{
OUT_PWD
}
BUILDDIR
=
$$
{
TARGETDIR
}
/
build
LANGUAGE
=
C
++
#
OBJECTS_DIR
=
$$
BUILDDIR
/
obj
#
MOC_DIR
=
$$
BUILDDIR
/
moc
#
UI_HEADERS_DIR
=
$$
BUILDDIR
/
ui
#
RCC_DIR
=
$$
BUILDDIR
/
rcc
OBJECTS_DIR
=
$$
{
BUILDDIR
}
/
obj
MOC_DIR
=
$$
{
BUILDDIR
}
/
moc
UI_DIR
=
$$
{
BUILDDIR
}
/
ui
RCC_DIR
=
$$
{
BUILDDIR
}
/
rcc
MAVLINK_CONF
=
""
DEFINES
+=
MAVLINK_NO_DATA
QMAKE_INCDIR_QT
=
$$
(
QTDIR
)
/
include
QMAKE_LIBDIR_QT
=
$$
(
QTDIR
)
/
lib
QMAKE_UIC
=
"$$(QTDIR)/bin/uic.exe"
QMAKE_MOC
=
"$$(QTDIR)/bin/moc.exe"
QMAKE_RCC
=
"$$(QTDIR)/bin/rcc.exe"
QMAKE_QMAKE
=
"$$(QTDIR)/bin/qmake.exe"
#################################################################
#
EXTERNAL
LIBRARY
CONFIGURATION
...
...
@@ -119,6 +129,16 @@ contains(MAVLINK_CONF, ardupilotmega) {
INCLUDEPATH
+=
$$
BASEDIR
/
thirdParty
/
mavlink
/
include
/
ardupilotmega
DEFINES
+=
QGC_USE_ARDUPILOTMEGA_MESSAGES
}
contains
(
MAVLINK_CONF
,
senseSoar
)
{
#
Remove
the
default
set
-
it
is
included
anyway
INCLUDEPATH
-=
$$
BASEDIR
/../
mavlink
/
include
/
common
INCLUDEPATH
-=
$$
BASEDIR
/
thirdParty
/
mavlink
/
include
/
common
#
SENSESOAR
SPECIAL
MESSAGES
INCLUDEPATH
+=
$$
BASEDIR
/../
mavlink
/
include
/
SenseSoar
INCLUDEPATH
+=
$$
BASEDIR
/
thirdParty
/
mavlink
/
include
/
SenseSoar
DEFINES
+=
QGC_USE_SENSESOAR_MESSAGES
}
#
Include
general
settings
for
QGroundControl
...
...
@@ -272,6 +292,7 @@ HEADERS += src/MG.h \
src
/
uas
/
SlugsMAV
.
h
\
src
/
uas
/
PxQuadMAV
.
h
\
src
/
uas
/
ArduPilotMegaMAV
.
h
\
src
/
uas
/
senseSoarMAV
.
h
\
src
/
ui
/
watchdog
/
WatchdogControl
.
h
\
src
/
ui
/
watchdog
/
WatchdogProcessView
.
h
\
src
/
ui
/
watchdog
/
WatchdogView
.
h
\
...
...
@@ -402,6 +423,7 @@ SOURCES += src/main.cc \
src
/
uas
/
SlugsMAV
.
cc
\
src
/
uas
/
PxQuadMAV
.
cc
\
src
/
uas
/
ArduPilotMegaMAV
.
cc
\
src
/
uas
/
senseSoarMAV
.
cpp
\
src
/
ui
/
watchdog
/
WatchdogControl
.
cc
\
src
/
ui
/
watchdog
/
WatchdogProcessView
.
cc
\
src
/
ui
/
watchdog
/
WatchdogView
.
cc
\
...
...
@@ -516,19 +538,21 @@ win32:exists(src/lib/opalrt/OpalApi.h):exists(C:/OPAL-RT/RT-LAB7.2.4/Common/bin)
TRANSLATIONS
+=
es
-
MX
.
ts
\
en
-
US
.
ts
##
xbee
support
##
libxbee
only
supported
by
linux
and
windows
systems
##
win32
-
msvc2008
|
win32
-
msvc2010
|
linux
{
#
HEADERS
+=
src
/
comm
/
XbeeLinkInterface
.
h
\
#
src
/
comm
/
XbeeLink
.
h
\
#
src
/
ui
/
XbeeConfigurationWindow
.
h
\
#
src
/
comm
/
CallConv
.
h
#
SOURCES
+=
src
/
comm
/
XbeeLink
.
cpp
\
#
src
/
ui
/
XbeeConfigurationWindow
.
cpp
#
DEFINES
+=
XBEELINK
#
INCLUDEPATH
+=
thirdParty
/
libxbee
##
TO
DO
:
build
library
when
it
does
not
exists
already
#
LIBS
+=
-
LthirdParty
/
libxbee
/
lib
\
#
-
llibxbee
#
#
}
#
xbee
support
#
libxbee
only
supported
by
linux
and
windows
systems
win32
-
msvc2008
|
win32
-
msvc2010
|
linux
{
HEADERS
+=
src
/
comm
/
XbeeLinkInterface
.
h
\
src
/
comm
/
XbeeLink
.
h
\
src
/
comm
/
HexSpinBox
.
h
\
src
/
ui
/
XbeeConfigurationWindow
.
h
\
src
/
comm
/
CallConv
.
h
SOURCES
+=
src
/
comm
/
XbeeLink
.
cpp
\
src
/
comm
/
HexSpinBox
.
cpp
\
src
/
ui
/
XbeeConfigurationWindow
.
cpp
DEFINES
+=
XBEELINK
INCLUDEPATH
+=
thirdParty
/
libxbee
#
TO
DO
:
build
library
when
it
does
not
exists
already
LIBS
+=
-
LthirdParty
/
libxbee
/
lib
\
-
llibxbee
}
src/QGCCore.cc
View file @
f3cd7d6c
...
...
@@ -143,6 +143,7 @@ QGCCore::QGCCore(int &argc, char* argv[]) : QApplication(argc, argv)
// to make sure that all components are initialized when the
// first messages arrive
UDPLink
*
udpLink
=
new
UDPLink
(
QHostAddress
::
Any
,
14550
);
MainWindow
::
instance
()
->
addLink
(
udpLink
);
// Listen on Multicast-Address 239.255.77.77, Port 14550
//QHostAddress * multicast_udp = new QHostAddress("239.255.77.77");
//UDPLink* udpLink = new UDPLink(*multicast_udp, 14550);
...
...
src/comm/LinkInterface.h
View file @
f3cd7d6c
...
...
@@ -237,7 +237,7 @@ signals:
protected:
static
int
getNextLinkId
()
{
static
int
nextId
=
0
;
static
int
nextId
=
1
;
return
nextId
++
;
}
...
...
src/comm/SerialLink.cc
View file @
f3cd7d6c
...
...
@@ -211,7 +211,7 @@ using namespace TNX;
SerialLink
::
SerialLink
(
QString
portname
,
int
baudRate
,
bool
hardwareFlowControl
,
bool
parity
,
int
dataBits
,
int
stopBits
)
:
port
(
NULL
),
port
(
NULL
),
m_stopp
(
false
),
ports
(
new
QVector
<
QString
>
())
{
// Setup settings
...
...
@@ -340,7 +340,7 @@ QVector<QString>* SerialLink::getCurrentPorts()
QextPortInfo
portInfo
=
ports
.
at
(
i
);
QString
portString
=
QString
(
portInfo
.
portName
.
toLocal8Bit
().
constData
())
+
" - "
+
QString
(
ports
.
at
(
i
).
friendName
.
toLocal8Bit
().
constData
()).
split
(
"("
).
first
();
// Prepend newly found port to the list
ports
.
append
(
portString
);
this
->
ports
->
append
(
portString
);
}
//printf("port name: %s\n", ports.at(i).portName.toLocal8Bit().constData());
...
...
@@ -349,7 +349,7 @@ QVector<QString>* SerialLink::getCurrentPorts()
//printf("enumerator name: %s\n", ports.at(i).enumName.toLocal8Bit().constData());
//printf("===================================\n\n");
#endif
return
ports
;
return
this
->
ports
;
}
void
SerialLink
::
loadSettings
()
...
...
@@ -394,6 +394,14 @@ void SerialLink::run()
// Qt way to make clear what a while(1) loop does
forever
{
{
QMutexLocker
locker
(
&
this
->
m_stoppMutex
);
if
(
this
->
m_stopp
)
{
this
->
m_stopp
=
false
;
break
;
}
}
// Check if new bytes have arrived, if yes, emit the notification signal
checkForBytes
();
/* Serial data isn't arriving that fast normally, this saves the thread
...
...
@@ -401,6 +409,13 @@ void SerialLink::run()
*/
MG
::
SLEEP
::
msleep
(
SerialLink
::
poll_interval
);
}
if
(
port
)
{
port
->
flushInBuffer
();
port
->
flushOutBuffer
();
port
->
close
();
delete
port
;
port
=
NULL
;
}
}
...
...
@@ -512,25 +527,34 @@ qint64 SerialLink::bytesAvailable()
**/
bool
SerialLink
::
disconnect
()
{
if
(
port
)
{
if
(
this
->
isRunning
())
{
{
QMutexLocker
locker
(
&
this
->
m_stoppMutex
);
this
->
m_stopp
=
true
;
}
this
->
wait
();
// if (port) {
//#if !defined _WIN32 || !defined _WIN64
/* Block the thread until it returns from run() */
//#endif
port
->
flushInBuffer
();
port
->
flushOutBuffer
();
port
->
close
();
delete
port
;
port
=
NULL
;
if
(
this
->
isRunning
())
this
->
terminate
();
//stop running the thread, restart it upon connect
//
port->flushInBuffer();
//
port->flushOutBuffer();
//
port->close();
//
delete port;
//
port = NULL;
//
if(this->isRunning()) this->terminate(); //stop running the thread, restart it upon connect
bool
closed
=
true
;
//port->isOpen();
emit
disconnected
();
emit
connected
(
false
);
return
closed
;
}
else
{
}
else
{
// No port, so we're disconnected
return
true
;
}
...
...
@@ -545,6 +569,10 @@ bool SerialLink::disconnect()
bool
SerialLink
::
connect
()
{
if
(
this
->
isRunning
())
this
->
disconnect
();
{
QMutexLocker
locker
(
&
this
->
m_stoppMutex
);
this
->
m_stopp
=
false
;
}
this
->
start
(
LowPriority
);
return
true
;
}
...
...
src/comm/SerialLink.h
View file @
f3cd7d6c
...
...
@@ -168,6 +168,10 @@ protected:
QMutex
dataMutex
;
QVector
<
QString
>*
ports
;
private:
volatile
bool
m_stopp
;
QMutex
m_stoppMutex
;
void
setName
(
QString
name
);
bool
hardwareConnect
();
...
...
src/comm/UDPLink.cc
View file @
f3cd7d6c
...
...
@@ -40,19 +40,22 @@ This file is part of the QGROUNDCONTROL project
//#include <netinet/in.h>
UDPLink
::
UDPLink
(
QHostAddress
host
,
quint16
port
)
:
socket
(
NULL
)
{
this
->
host
=
host
;
this
->
port
=
port
;
this
->
connectState
=
false
;
// Set unique ID and add link to the list of links
this
->
id
=
getNextLinkId
();
this
->
name
=
tr
(
"UDP Link (port:%1)"
).
arg
(
14550
);
LinkManager
::
instance
()
->
add
(
this
);
this
->
name
=
tr
(
"UDP Link (port:%1)"
).
arg
(
this
->
port
);
emit
nameChanged
(
this
->
name
);
// LinkManager::instance()->add(this);
}
UDPLink
::~
UDPLink
()
{
disconnect
();
this
->
deleteLater
();
}
/**
...
...
@@ -61,23 +64,39 @@ UDPLink::~UDPLink()
**/
void
UDPLink
::
run
()
{
// forever
// {
// QGC::SLEEP::msleep(5000);
// }
exec
();
exec
();
}
void
UDPLink
::
setAddress
(
Q
String
address
)
void
UDPLink
::
setAddress
(
Q
HostAddress
host
)
{
Q_UNUSED
(
address
);
bool
reconnect
(
false
);
if
(
this
->
isConnected
())
{
disconnect
();
reconnect
=
true
;
}
this
->
host
=
host
;
if
(
reconnect
)
{
connect
();
}
}
void
UDPLink
::
setPort
(
int
port
)
{
bool
reconnect
(
false
);
if
(
this
->
isConnected
())
{
disconnect
();
reconnect
=
true
;
}
this
->
port
=
port
;
disconnect
();
connect
();
this
->
name
=
tr
(
"UDP Link (port:%1)"
).
arg
(
this
->
port
);
emit
nameChanged
(
this
->
name
);
if
(
reconnect
)
{
connect
();
}
}
/**
...
...
@@ -104,9 +123,11 @@ void UDPLink::addHost(const QString& host)
}
}
hosts
.
append
(
address
);
this
->
setAddress
(
address
);
//qDebug() << "Address:" << address.toString();
// Set port according to user input
ports
.
append
(
host
.
split
(
":"
).
last
().
toInt
());
this
->
setPort
(
host
.
split
(
":"
).
last
().
toInt
());
}
}
else
...
...
@@ -245,8 +266,14 @@ qint64 UDPLink::bytesAvailable()
**/
bool
UDPLink
::
disconnect
()
{
delete
socket
;
socket
=
NULL
;
this
->
quit
();
this
->
wait
();
if
(
socket
)
{
delete
socket
;
socket
=
NULL
;
}
connectState
=
false
;
...
...
@@ -262,7 +289,19 @@ bool UDPLink::disconnect()
**/
bool
UDPLink
::
connect
()
{
socket
=
new
QUdpSocket
(
this
);
if
(
this
->
isRunning
())
{
this
->
quit
();
this
->
wait
();
}
this
->
hardwareConnect
();
start
(
HighPriority
);
return
true
;
}
bool
UDPLink
::
hardwareConnect
(
void
)
{
socket
=
new
QUdpSocket
();
//Check if we are using a multicast-address
// bool multicast = false;
...
...
@@ -309,11 +348,10 @@ bool UDPLink::connect()
emit
connected
();
connectionStartTime
=
QGC
::
groundTimeUsecs
()
/
1000
;
}
start
(
HighPriority
);
return
connectState
;
return
connectState
;
}
/**
* @brief Check if connection is active.
*
...
...
src/comm/UDPLink.h
View file @
f3cd7d6c
...
...
@@ -87,7 +87,7 @@ public:
int
getId
();
public
slots
:
void
setAddress
(
Q
String
address
);
void
setAddress
(
Q
HostAddress
host
);
void
setPort
(
int
port
);
/** @brief Add a new host to broadcast messages to */
void
addHost
(
const
QString
&
host
);
...
...
@@ -128,8 +128,11 @@ protected:
void
setName
(
QString
name
);
private:
bool
hardwareConnect
(
void
);
signals:
//
Signals are defined by LinkInterface
//Signals are defined by LinkInterface
};
...
...
src/comm/XbeeLink.cpp
View file @
f3cd7d6c
...
...
@@ -7,7 +7,8 @@
#include "XbeeLink.h"
XbeeLink
::
XbeeLink
(
QString
portName
,
int
baudRate
)
:
m_xbeeCon
(
NULL
),
m_portName
(
NULL
),
m_portNameLength
(
0
),
m_baudRate
(
baudRate
),
m_connected
(
false
),
m_id
(
-
1
)
m_xbeeCon
(
NULL
),
m_portName
(
NULL
),
m_portNameLength
(
0
),
m_baudRate
(
baudRate
),
m_connected
(
false
),
m_id
(
-
1
),
m_addrHigh
(
0
),
m_addrLow
(
0
)
{
/* setup the xbee */
...
...
@@ -160,6 +161,7 @@ qint64 XbeeLink::getBitsReceived()
bool
XbeeLink
::
hardwareConnect
()
{
emit
tryConnectBegin
(
true
);
if
(
this
->
isConnected
())
{
this
->
disconnect
();
...
...
@@ -172,9 +174,11 @@ bool XbeeLink::hardwareConnect()
{
/* oh no... it failed */
qDebug
()
<<
"xbee_setup() failed...
\n
"
;
emit
tryConnectEnd
(
true
);
return
false
;
}
this
->
m_xbeeCon
=
xbee_newcon
(
'A'
,
xbee2_data
,
0x13A200
,
0x403D0935
);
emit
tryConnectEnd
(
true
);
this
->
m_connected
=
true
;
emit
connected
();
emit
connected
(
true
);
...
...
@@ -194,7 +198,7 @@ bool XbeeLink::disconnect()
if
(
this
->
m_xbeeCon
)
{
xbee_end
con
(
this
->
m_xbeeCon
);
xbee_end
(
);
this
->
m_xbeeCon
=
NULL
;
}
this
->
m_connected
=
false
;
...
...
@@ -256,6 +260,18 @@ void XbeeLink::run()
}
}
bool
XbeeLink
::
setRemoteAddressHigh
(
quint32
high
)
{
this
->
m_addrHigh
=
high
;
return
true
;
}
bool
XbeeLink
::
setRemoteAddressLow
(
quint32
low
)
{
this
->
m_addrLow
=
low
;
return
true
;
}
/*
void CALLTYPE XbeeLink::portCallback(xbee_con *xbeeCon, xbee_pkt *XbeePkt)
{
...
...
src/comm/XbeeLink.h
View file @
f3cd7d6c
...
...
@@ -26,6 +26,8 @@ public: // virtual functions from XbeeLinkInterface
public
slots
:
// virtual functions from XbeeLinkInterface
bool
setPortName
(
QString
portName
);
bool
setBaudRate
(
int
rate
);
bool
setRemoteAddressHigh
(
quint32
high
);
bool
setRemoteAddressLow
(
quint32
low
);
public:
// virtual functions from LinkInterface
int
getId
();
...
...
@@ -60,6 +62,8 @@ protected:
int
m_baudRate
;
bool
m_connected
;
QString
m_name
;
quint32
m_addrHigh
;
quint32
m_addrLow
;
private:
bool
hardwareConnect
();
...
...
src/comm/XbeeLinkInterface.h
View file @
f3cd7d6c
...
...
@@ -16,6 +16,12 @@ public:
public
slots
:
virtual
bool
setPortName
(
QString
portName
)
=
0
;
virtual
bool
setBaudRate
(
int
rate
)
=
0
;
virtual
bool
setRemoteAddressHigh
(
quint32
high
)
=
0
;
virtual
bool
setRemoteAddressLow
(
quint32
low
)
=
0
;
signals:
void
tryConnectBegin
(
bool
toTrue
);
void
tryConnectEnd
(
bool
toTrue
);
};
#endif // XBEELINKINTERFACE_H_
src/uas/QGCMAVLinkUASFactory.cc
View file @
f3cd7d6c
...
...
@@ -72,6 +72,16 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
uas
=
mav
;
}
break
;
#ifdef QGC_USE_SENSESOAR_MESSAGES
case
MAV_AUTOPILOT_SENSESOAR
:
{
senseSoarMAV
*
mav
=
new
senseSoarMAV
(
mavlink
,
sysid
);
mav
->
setSystemType
((
int
)
heartbeat
->
type
);
connect
(
mavlink
,
SIGNAL
(
messageReceived
(
LinkInterface
*
,
mavlink_message_t
)),
mav
,
SLOT
(
receiveMessage
(
LinkInterface
*
,
mavlink_message_t
)));
uas
=
mav
;
break
;
}
#endif
default:
{
UAS
*
mav
=
new
UAS
(
mavlink
,
sysid
);
...
...
src/uas/QGCMAVLinkUASFactory.h
View file @
f3cd7d6c
...
...
@@ -12,6 +12,7 @@
#include "UAS.h"
#include "SlugsMAV.h"
#include "PxQuadMAV.h"
#include "senseSoarMAV.h"
#include "ArduPilotMegaMAV.h"
class
QGCMAVLinkUASFactory
:
public
QObject
...
...
src/uas/UAS.h
View file @
f3cd7d6c
...
...
@@ -521,16 +521,16 @@ public slots:
signals:
/** @brief The main/battery voltage has changed/was updated */
void
voltageChanged
(
int
uasId
,
double
voltage
);
//void voltageChanged(int uasId, double voltage); // Defined in UASInterface already
/** @brief An actuator value has changed */
void
actuatorChanged
(
UASInterface
*
,
int
actId
,
double
value
);
//void actuatorChanged(UASInterface*, int actId, double value); // Defined in UASInterface already
/** @brief An actuator value has changed */
void
actuatorChanged
(
UASInterface
*
uas
,
QString
actuatorName
,
double
min
,
double
max
,
double
value
);
void
motorChanged
(
UASInterface
*
uas
,
QString
motorName
,
double
min
,
double
max
,
double
value
);
/** @brief The system load (MCU/CPU usage) changed */
void
loadChanged
(
UASInterface
*
uas
,
double
load
);
/** @brief Propagate a heartbeat received from the system */
void
heartbeat
(
UASInterface
*
uas
);
//void heartbeat(UASInterface* uas); // Defined in UASInterface already
void
imageStarted
(
quint64
timestamp
);
/** @brief A new camera image has arrived */
void
imageReady
(
UASInterface
*
uas
);
...
...
src/uas/senseSoarMAV.cpp
0 → 100644
View file @
f3cd7d6c
#include "senseSoarMAV.h"
#include <qmath.h>
senseSoarMAV
::
senseSoarMAV
(
MAVLinkProtocol
*
mavlink
,
int
id
)
:
UAS
(
mavlink
,
id
),
senseSoarState
(
0
)
{
}
senseSoarMAV
::~
senseSoarMAV
(
void
)
{
}
void
senseSoarMAV
::
receiveMessage
(
LinkInterface
*
link
,
mavlink_message_t
message
)
{
#ifdef MAVLINK_ENABLED_SENSESOAR
if
(
message
.
sysid
==
uasId
)
// make sure the message is for the right UAV
{
if
(
!
link
)
return
;
switch
(
message
.
msgid
)
{
case
MAVLINK_MSG_ID_CMD_AIRSPEED_ACK
:
// TO DO: check for acknowledgement after sended commands
{
mavlink_cmd_airspeed_ack_t
airSpeedMsg
;
mavlink_msg_cmd_airspeed_ack_decode
(
&
message
,
&
airSpeedMsg
);
break
;
}
/*case MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG: only sent to UAV
{
break;
}*/
case
MAVLINK_MSG_ID_FILT_ROT_VEL
:
// rotational velocities
{
mavlink_filt_rot_vel_t
rotVelMsg
;
mavlink_msg_filt_rot_vel_decode
(
&
message
,
&
rotVelMsg
);
quint64
time
=
getUnixTime
();
for
(
unsigned
char
i
=
0
;
i
<
3
;
i
++
)
{
this
->
m_rotVel
[
i
]
=
rotVelMsg
.
rotVel
[
i
];
}
emit
valueChanged
(
uasId
,
"rollspeed"
,
"rad/s"
,
this
->
m_rotVel
[
0
],
time
);
emit
valueChanged
(
uasId
,
"pitchspeed"
,
"rad/s"
,
this
->
m_rotVel
[
1
],
time
);
emit
valueChanged
(
uasId
,
"yawspeed"
,
"rad/s"
,
this
->
m_rotVel
[
2
],
time
);
emit
attitudeSpeedChanged
(
uasId
,
this
->
m_rotVel
[
0
],
this
->
m_rotVel
[
1
],
this
->
m_rotVel
[
2
],
time
);
break
;
}
case
MAVLINK_MSG_ID_LLC_OUT
:
// low level controller output
{
mavlink_llc_out_t
llcMsg
;
mavlink_msg_llc_out_decode
(
&
message
,
&
llcMsg
);
quint64
time
=
getUnixTime
();
emit
valueChanged
(
uasId
,
"Servo. 1"
,
"rad"
,
llcMsg
.
servoOut
[
0
],
time
);
emit
valueChanged
(
uasId
,
"Servo. 2"
,
"rad"
,
llcMsg
.
servoOut
[
1
],
time
);
emit
valueChanged
(
uasId
,
"Servo. 3"
,
"rad"
,
llcMsg
.
servoOut
[
2
],
time
);
emit
valueChanged
(
uasId
,
"Servo. 4"
,
"rad"
,
llcMsg
.
servoOut
[
3
],
time
);
emit
valueChanged
(
uasId
,
"Motor. 1"
,
"raw"
,
llcMsg
.
MotorOut
[
0
]
,
time
);
emit
valueChanged
(
uasId
,
"Motor. 2"
,
"raw"
,
llcMsg
.
MotorOut
[
1
],
time
);
break
;
}
case
MAVLINK_MSG_ID_OBS_AIR_TEMP
:
{
mavlink_obs_air_temp_t
airTMsg
;
mavlink_msg_obs_air_temp_decode
(
&
message
,
&
airTMsg
);
quint64
time
=
getUnixTime
();
emit
valueChanged
(
uasId
,
"Air Temp"
,
""
,
airTMsg
.
airT
,
time
);
break
;
}
case
MAVLINK_MSG_ID_OBS_AIR_VELOCITY
:
{
mavlink_obs_air_velocity_t
airVMsg
;
mavlink_msg_obs_air_velocity_decode
(
&
message
,
&
airVMsg
);
quint64
time
=
getUnixTime
();
emit
valueChanged
(
uasId
,
"AirVel. mag"
,
"m/s"
,
airVMsg
.
magnitude
,
time
);
emit
valueChanged
(
uasId
,
"AirVel. AoA"
,
"rad"
,
airVMsg
.
aoa
,
time
);
emit
valueChanged
(
uasId
,
"AirVel. Slip"
,
"rad"
,
airVMsg
.
slip
,
time
);
break
;
}
case
MAVLINK_MSG_ID_OBS_ATTITUDE
:
{
mavlink_obs_attitude_t
quatMsg
;
mavlink_msg_obs_attitude_decode
(
&
message
,
&
quatMsg
);
quint64
time
=
getUnixTime
();
this
->
quat2euler
(
quatMsg
.
quat
,
this
->
roll
,
this
->
pitch
,
this
->
yaw
);
emit
valueChanged
(
uasId
,
"roll"
,
"rad"
,
roll
,
time
);
emit
valueChanged
(
uasId
,
"pitch"
,
"rad"
,
pitch
,
time
);
emit
valueChanged
(
uasId
,
"yaw"
,
"rad"
,
yaw
,
time
);
emit
valueChanged
(
uasId
,
"roll deg"
,
"deg"
,
(
roll
/
M_PI
)
*
180.0
,
time
);
emit
valueChanged
(
uasId
,
"pitch deg"
,
"deg"
,
(
pitch
/
M_PI
)
*
180.0
,
time
);
emit
valueChanged
(
uasId
,
"heading deg"
,
"deg"
,
(
yaw
/
M_PI
)
*
180.0
,
time
);
emit
attitudeChanged
(
this
,
roll
,
pitch
,
yaw
,
time
);
break
;
}
case
MAVLINK_MSG_ID_OBS_BIAS
:
{
mavlink_obs_bias_t
biasMsg
;
mavlink_msg_obs_bias_decode
(
&
message
,
&
biasMsg
);
quint64
time
=
getUnixTime
();
emit
valueChanged
(
uasId
,
"acc. biasX"
,
"m/s^2"
,
biasMsg
.
accBias
[
0
],
time
);
emit
valueChanged
(
uasId
,
"acc. biasY"
,
"m/s^2"
,
biasMsg
.
accBias
[
1
],
time
);
emit
valueChanged
(
uasId
,
"acc. biasZ"
,
"m/s^2"
,
biasMsg
.
accBias
[
2
],
time
);
emit
valueChanged
(
uasId
,
"gyro. biasX"
,
"rad/s"
,
biasMsg
.
gyroBias
[
0
],
time
);
emit
valueChanged
(
uasId
,
"gyro. biasY"
,
"rad/s"
,
biasMsg
.
gyroBias
[
1
],
time
);
emit
valueChanged
(
uasId
,
"gyro. biasZ"
,
"rad/s"
,
biasMsg
.
gyroBias
[
2
],
time
);
break
;
}
case
MAVLINK_MSG_ID_OBS_POSITION
:
{
mavlink_obs_position_t
posMsg
;
mavlink_msg_obs_position_decode
(
&
message
,
&
posMsg
);
quint64
time
=
getUnixTime
();
this
->
longitude
=
posMsg
.
lon
/
(
double
)
1E7
;
this
->
latitude
=
posMsg
.
lat
/
(
double
)
1E7
;
this
->
altitude
=
posMsg
.
alt
/
1000.0
;
emit
valueChanged
(
uasId
,
"latitude"
,
"deg"
,
this
->
latitude
,
time
);
emit
valueChanged
(
uasId
,
"longitude"
,
"deg"
,
this
->
longitude
,
time
);
emit
valueChanged
(
uasId
,
"altitude"
,
"m"
,
this
->
altitude
,
time
);
emit
globalPositionChanged
(
this
,
this
->
latitude
,
this
->
longitude
,
this
->
altitude
,
time
);
break
;
}
case
MAVLINK_MSG_ID_OBS_QFF
:
{
mavlink_obs_qff_t
qffMsg
;
mavlink_msg_obs_qff_decode
(
&
message
,
&
qffMsg
);
quint64
time
=
getUnixTime
();
emit
valueChanged
(
uasId
,
"QFF"
,
"Pa"
,
qffMsg
.
qff
,
time
);
break
;
}
case
MAVLINK_MSG_ID_OBS_VELOCITY
:
{
mavlink_obs_velocity_t
velMsg
;