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
c8fe3480
Commit
c8fe3480
authored
Jan 13, 2011
by
lm
Browse files
Options
Browse Files
Download
Plain Diff
Merge branch 'experimental' of github.com:pixhawk/qgroundcontrol into experimental
parents
fd6c038e
05b444a8
Changes
22
Show whitespace changes
Inline
Side-by-side
Showing
22 changed files
with
2201 additions
and
1689 deletions
+2201
-1689
qgcunittest.pro
qgcunittest.pro
+137
-0
tst_uasunittest.cc
qgcunittest/tst_uasunittest.cc
+186
-0
qgroundcontrol.pri
qgroundcontrol.pri
+7
-5
qgroundcontrol.pro
qgroundcontrol.pro
+1
-0
GAudioOutput.h
src/GAudioOutput.h
+2
-2
MAVLinkProtocol.cc
src/comm/MAVLinkProtocol.cc
+2
-1
SlugsMAV.cc
src/uas/SlugsMAV.cc
+64
-57
SlugsMAV.h
src/uas/SlugsMAV.h
+5
-0
UAS.h
src/uas/UAS.h
+1
-1
UASWaypointManager.cc
src/uas/UASWaypointManager.cc
+6
-1
MainWindow.cc
src/ui/MainWindow.cc
+42
-27
MapWidget.cc
src/ui/MapWidget.cc
+2
-1
SlugsDataSensorView.cc
src/ui/SlugsDataSensorView.cc
+3
-3
SlugsDataSensorView.ui
src/ui/SlugsDataSensorView.ui
+1510
-1522
SlugsHilSim.cc
src/ui/SlugsHilSim.cc
+160
-25
SlugsHilSim.h
src/ui/SlugsHilSim.h
+22
-1
SlugsHilSim.ui
src/ui/SlugsHilSim.ui
+14
-1
SlugsPIDControl.cpp
src/ui/SlugsPIDControl.cpp
+7
-7
SlugsPadCameraControl.cpp
src/ui/SlugsPadCameraControl.cpp
+13
-20
SlugsPadCameraControl.h
src/ui/SlugsPadCameraControl.h
+4
-0
SlugsVideoCamControl.ui
src/ui/SlugsVideoCamControl.ui
+11
-15
QGCParamSlider.cc
src/ui/designer/QGCParamSlider.cc
+2
-0
No files found.
qgcunittest.pro
0 → 100644
View file @
c8fe3480
#-------------------------------------------------
#
#
Project
created
by
QtCreator
2011
-
01
-
11
T08
:
54
:
37
#
#-------------------------------------------------
QT
+=
network
\
phonon
\
testlib
\
svg
TEMPLATE
=
app
TARGET
=
tst_uasunittest
BASEDIR
=
$$
IN_PWD
TESTDIR
=
$$
BASEDIR
/
qgcunittest
TARGETDIR
=
$$
OUT_PWD
BUILDDIR
=
$$
TARGETDIR
/
build
LANGUAGE
=
C
++
CONFIG
+=
console
CONFIG
-=
app_bundle
OBJECTS_DIR
=
$$
BUILDDIR
/
obj
MOC_DIR
=
$$
BUILDDIR
/
moc
UI_HEADERS_DIR
=
src
/
ui
/
generated
MAVLINK_CONF
=
""
#
If
the
user
config
file
exists
,
it
will
be
included
.
#
if
the
variable
MAVLINK_CONF
contains
the
name
of
an
#
additional
project
,
QGroundControl
includes
the
support
#
of
custom
MAVLink
messages
of
this
project
exists
(
user_config
.
pri
)
{
include
(
user_config
.
pri
)
message
(
"----- USING CUSTOM USER QGROUNDCONTROL CONFIG FROM user_config.pri -----"
)
message
(
"Adding support for additional MAVLink messages for: "
$$
MAVLINK_CONF
)
message
(
"------------------------------------------------------------------------"
)
}
INCLUDEPATH
+=
$$
BASEDIR
/../
mavlink
/
include
/
common
contains
(
MAVLINK_CONF
,
pixhawk
)
{
#
Remove
the
default
set
-
it
is
included
anyway
INCLUDEPATH
-=
$$
BASEDIR
/../
mavlink
/
include
/
common
#
PIXHAWK
SPECIAL
MESSAGES
INCLUDEPATH
+=
$$
BASEDIR
/../
mavlink
/
include
/
pixhawk
DEFINES
+=
QGC_USE_PIXHAWK_MESSAGES
}
contains
(
MAVLINK_CONF
,
slugs
)
{
#
Remove
the
default
set
-
it
is
included
anyway
INCLUDEPATH
-=
$$
BASEDIR
/../
mavlink
/
include
/
common
#
SLUGS
SPECIAL
MESSAGES
INCLUDEPATH
+=
$$
BASEDIR
/../
mavlink
/
include
/
slugs
DEFINES
+=
QGC_USE_SLUGS_MESSAGES
}
contains
(
MAVLINK_CONF
,
ualberta
)
{
#
Remove
the
default
set
-
it
is
included
anyway
INCLUDEPATH
-=
$$
BASEDIR
/../
mavlink
/
include
/
common
#
UALBERTA
SPECIAL
MESSAGES
INCLUDEPATH
+=
$$
BASEDIR
/../
mavlink
/
include
/
ualberta
DEFINES
+=
QGC_USE_UALBERTA_MESSAGES
}
contains
(
MAVLINK_CONF
,
ardupilotmega
)
{
#
Remove
the
default
set
-
it
is
included
anyway
INCLUDEPATH
-=
$$
BASEDIR
/../
mavlink
/
include
/
common
#
UALBERTA
SPECIAL
MESSAGES
INCLUDEPATH
+=
$$
BASEDIR
/../
mavlink
/
include
/
ardupilotmega
DEFINES
+=
QGC_USE_ARDUPILOTMEGA_MESSAGES
}
#
Include
general
settings
for
MAVGround
#
necessary
as
last
include
to
override
any
non
-
acceptable
settings
#
done
by
the
plugins
above
include
(
qgroundcontrol
.
pri
)
#
QWT
plot
and
QExtSerial
depend
on
paths
set
by
qgroundcontrol
.
pri
#
Include
serial
port
library
include
(
src
/
lib
/
qextserialport
/
qextserialport
.
pri
)
#
Include
QWT
plotting
library
include
(
src
/
lib
/
qwt
/
qwt
.
pri
)
DEPENDPATH
+=
.
\
lib
/
QMapControl
\
lib
/
QMapControl
/
src
\
plugins
INCLUDEPATH
+=
.
\
lib
/
QMapControl
\
$$
BASEDIR
/../
mavlink
/
include
\
$$
BASEDIR
/
src
/
uas
\
$$
BASEDIR
/
src
/
comm
\
$$
BASEDIR
/
src
/
\
$$
BASEDIR
/
src
/
ui
/
RadioCalibration
\
$$
BASEDIR
/
src
/
ui
/
\
SOURCES
+=
$$
TESTDIR
/
tst_uasunittest
.
cc
\
src
/
uas
/
UAS
.
cc
\
src
/
comm
/
MAVLinkProtocol
.
cc
\
src
/
uas
/
UASWaypointManager
.
cc
\
src
/
Waypoint
.
cc
\
src
/
ui
/
RadioCalibration
/
RadioCalibrationData
.
cc
\
src
/
uas
/
SlugsMAV
.
cc
\
src
/
uas
/
PxQuadMAV
.
cc
\
src
/
uas
/
ArduPilotMegaMAV
.
cc
\
src
/
GAudioOutput
.
cc
\
src
/
uas
/
UASManager
.
cc
\
src
/
comm
/
LinkManager
.
cc
\
src
/
QGC
.
cc
\
src
/
comm
/
SerialLink
.
cc
\
HEADERS
+=
src
/
uas
/
UASInterface
.
h
\
src
/
uas
/
UAS
.
h
\
src
/
comm
/
MAVLinkProtocol
.
h
\
src
/
comm
/
ProtocolInterface
.
h
\
src
/
uas
/
UASWaypointManager
.
h
\
src
/
Waypoint
.
h
\
src
/
ui
/
RadioCalibration
/
RadioCalibrationData
.
h
\
src
/
uas
/
SlugsMAV
.
h
\
src
/
uas
/
PxQuadMAV
.
h
\
src
/
uas
/
ArduPilotMegaMAV
.
h
\
src
/
GAudioOutput
.
h
\
src
/
uas
/
UASManager
.
h
\
src
/
comm
/
LinkManager
.
h
\
src
/
comm
/
LinkInterface
.
h
\
src
/
QGC
.
h
\
src
/
comm
/
SerialLinkInterface
.
h
\
src
/
comm
/
SerialLink
.
h
\
DEFINES
+=
SRCDIR
=
\\\
"$$PWD/\\\"
qgcunittest/tst_uasunittest.cc
0 → 100644
View file @
c8fe3480
#include <QtCore/QString>
#include <QtTest/QtTest>
#include "UAS.h"
#include "MAVLinkProtocol.h"
#include "UASInterface.h"
class
UASUnitTest
:
public
QObject
{
Q_OBJECT
public:
#define UASID 50
MAVLinkProtocol
*
mav
;
UAS
*
uas
;
UASUnitTest
();
private
Q_SLOTS
:
void
initTestCase
();
void
cleanupTestCase
();
void
getUASID_test
();
void
getUASName_test
();
void
getUpTime_test
();
void
getCommunicationStatus_test
();
void
filterVoltage_test
();
void
getAutopilotType_test
();
void
setAutopilotType_test
();
void
getStatusForCode_test
();
void
getLocalX_test
();
void
getLocalY_test
();
void
getLocalZ_test
();
void
getLatitude_test
();
void
getLongitude_test
();
void
getAltitude_test
();
void
getRoll_test
();
void
getPitch_test
();
void
getYaw_test
();
void
calculateTimeRemaining_test
();
private:
protected:
UAS
*
prueba
;
};
UASUnitTest
::
UASUnitTest
()
{
}
void
UASUnitTest
::
initTestCase
()
{
mav
=
new
MAVLinkProtocol
();
uas
=
new
UAS
(
mav
,
UASID
);
}
void
UASUnitTest
::
cleanupTestCase
()
{
delete
uas
;
delete
mav
;
}
void
UASUnitTest
::
getUASID_test
()
{
// Test a default ID of zero is assigned
UAS
*
uas2
=
new
UAS
(
mav
);
QCOMPARE
(
uas2
->
getUASID
(),
0
);
delete
uas2
;
// Test that the chosen ID was assigned at construction
QCOMPARE
(
uas
->
getUASID
(),
UASID
);
// Make sure that no other ID was sert
QEXPECT_FAIL
(
""
,
"When you set an ID it does not use the default ID of 0"
,
Continue
);
QCOMPARE
(
uas
->
getUASID
(),
0
);
}
void
UASUnitTest
::
getUASName_test
()
{
// Test that the name is build as MAV + ID
QCOMPARE
(
uas
->
getUASName
(),
"MAV 0"
+
QString
::
number
(
UASID
));
}
void
UASUnitTest
::
getUpTime_test
()
{
UAS
*
uas2
=
new
UAS
(
mav
);
// Test that the uptime starts at zero to a
// precision of seconds
QCOMPARE
(
floor
(
uas2
->
getUptime
()
/
1000.0
),
0.0
);
// Sleep for three seconds
QTest
::
qSleep
(
3000
);
// Test that the up time is computed correctly to a
// precision of seconds
QCOMPARE
(
floor
(
uas2
->
getUptime
()
/
1000.0
),
3.0
);
delete
uas2
;
}
void
UASUnitTest
::
getCommunicationStatus_test
()
{
// Verify that upon construction the Comm status is disconnected
QCOMPARE
(
uas
->
getCommunicationStatus
(),
static_cast
<
int
>
(
UASInterface
::
COMM_DISCONNECTED
));
}
void
UASUnitTest
::
filterVoltage_test
()
{
float
verificar
=
uas
->
filterVoltage
(
0.4
f
);
// Verify that upon construction the Comm status is disconnected
QCOMPARE
(
verificar
,
8.52
f
);
}
void
UASUnitTest
::
getAutopilotType_test
()
{
int
verificar
=
uas
->
getAutopilotType
();
// Verify that upon construction the Comm status is disconnected
QCOMPARE
(
verificar
,
-
1
);
}
void
UASUnitTest
::
setAutopilotType_test
()
{
uas
->
setAutopilotType
(
2
);
// Verify that upon construction the Comm status is disconnected
QCOMPARE
(
uas
->
getAutopilotType
(),
2
);
}
void
UASUnitTest
::
getStatusForCode_test
()
{
// Verify that upon construction the Comm status is disconnected
QCOMPARE
(
uas
->
getYaw
(),
0.0
);
}
void
UASUnitTest
::
getLocalX_test
()
{
// Verify that upon construction the Comm status is disconnected
QCOMPARE
(
uas
->
getLocalX
(),
0.0
);
}
void
UASUnitTest
::
getLocalY_test
()
{
// Verify that upon construction the Comm status is disconnected
QCOMPARE
(
uas
->
getLocalY
(),
0.0
);
}
void
UASUnitTest
::
getLocalZ_test
()
{
// Verify that upon construction the Comm status is disconnected
QCOMPARE
(
uas
->
getLocalZ
(),
0.0
);
}
void
UASUnitTest
::
getLatitude_test
()
{
// Verify that upon construction the Comm status is disconnected
QCOMPARE
(
uas
->
getLatitude
(),
0.0
);
}
void
UASUnitTest
::
getLongitude_test
()
{
// Verify that upon construction the Comm status is disconnected
QCOMPARE
(
uas
->
getLongitude
(),
0.0
);
}
void
UASUnitTest
::
getAltitude_test
()
{
// Verify that upon construction the Comm status is disconnected
QCOMPARE
(
uas
->
getAltitude
(),
0.0
);
}
void
UASUnitTest
::
getRoll_test
()
{
// Verify that upon construction the Comm status is disconnected
QCOMPARE
(
uas
->
getRoll
(),
0.0
);
}
void
UASUnitTest
::
getPitch_test
()
{
// Verify that upon construction the Comm status is disconnected
QCOMPARE
(
uas
->
getPitch
(),
0.0
);
}
void
UASUnitTest
::
getYaw_test
()
{
// Verify that upon construction the Comm status is disconnected
QCOMPARE
(
uas
->
getYaw
(),
0.0
);
}
void
UASUnitTest
::
calculateTimeRemaining_test
()
{
/*
*/
}
QTEST_APPLESS_MAIN
(
UASUnitTest
);
#include "tst_uasunittest.moc"
qgroundcontrol.pri
View file @
c8fe3480
...
...
@@ -86,12 +86,12 @@ macx {
ICON = $$BASEDIR/images/icons/macx.icns
# Copy audio files if needed
QMAKE_POST_LINK += && cp -rf $$BASEDIR/audio $$TARGETDIR/qgroundcontrol.app/Contents/MacO
s
QMAKE_POST_LINK += && cp -rf $$BASEDIR/audio $$TARGETDIR/qgroundcontrol.app/Contents/MacO
S
# Copy google earth starter file
QMAKE_POST_LINK += && cp -f $$BASEDIR/images/earth.html $$TARGETDIR/qgroundcontrol.app/Contents/MacO
s
QMAKE_POST_LINK += && cp -f $$BASEDIR/images/earth.html $$TARGETDIR/qgroundcontrol.app/Contents/MacO
S
# Copy CSS stylesheets
QMAKE_POST_LINK += && cp -f $$BASEDIR/images/style-mission.css $$TARGETDIR/qgroundcontrol.app/Contents/MacO
s
/style-indoor.css
QMAKE_POST_LINK += && cp -f $$BASEDIR/images/style-outdoor.css $$TARGETDIR/qgroundcontrol.app/Contents/MacO
s
QMAKE_POST_LINK += && cp -f $$BASEDIR/images/style-mission.css $$TARGETDIR/qgroundcontrol.app/Contents/MacO
S
/style-indoor.css
QMAKE_POST_LINK += && cp -f $$BASEDIR/images/style-outdoor.css $$TARGETDIR/qgroundcontrol.app/Contents/MacO
S
# Copy model files
#QMAKE_POST_LINK += && cp -f $$BASEDIR/models/*.dae $$TARGETDIR/qgroundcontrol.app/Contents/MacOs
...
...
@@ -171,6 +171,8 @@ linux-g++ {
QMAKE_POST_LINK += cp -rf $$BASEDIR/audio $$DESTDIR/.
message("Compiling for linux 32")
INCLUDEPATH += /usr/include \
/usr/local/include \
/usr/include/qt4/phonon
...
...
qgroundcontrol.pro
View file @
c8fe3480
...
...
@@ -54,6 +54,7 @@ exists(user_config.pri) {
message
(
"Adding support for additional MAVLink messages for: "
$$
MAVLINK_CONF
)
message
(
"------------------------------------------------------------------------"
)
}
INCLUDEPATH
+=
$$
BASEDIR
/../
mavlink
/
include
/
common
contains
(
MAVLINK_CONF
,
pixhawk
)
{
#
Remove
the
default
set
-
it
is
included
anyway
...
...
src/GAudioOutput.h
View file @
c8fe3480
...
...
@@ -40,8 +40,8 @@ This file is part of the PIXHAWK project
#endif
#ifdef Q_OS_LINUX
//#include <flite/flite.h>
#include <
P
honon/MediaObject>
#include <
P
honon/AudioOutput>
#include <
p
honon/MediaObject>
#include <
p
honon/AudioOutput>
#endif
#ifdef Q_OS_WIN
#include <Phonon/MediaObject>
...
...
src/comm/MAVLinkProtocol.cc
View file @
c8fe3480
...
...
@@ -26,7 +26,7 @@
#include "ArduPilotMegaMAV.h"
#include "configuration.h"
#include "LinkManager.h"
#include "MainWindow.h"
//
#include "MainWindow.h"
#include <QGCMAVLink.h>
#include "QGC.h"
...
...
@@ -242,6 +242,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
}
else
{
// TODO: This if-else block can (should) be greatly simplified
if
(
lastIndex
[
message
.
sysid
][
message
.
compid
]
==
255
)
{
lastIndex
[
message
.
sysid
][
message
.
compid
]
=
0
;
...
...
src/uas/SlugsMAV.cc
View file @
c8fe3480
...
...
@@ -70,6 +70,10 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
#ifdef MAVLINK_ENABLED_SLUGS
case
MAVLINK_MSG_ID_RAW_IMU
:
mavlink_msg_raw_imu_decode
(
&
message
,
&
mlRawImuData
);
break
;
case
MAVLINK_MSG_ID_BOOT
:
mavlink_msg_boot_decode
(
&
message
,
&
mlBoot
);
emit
slugsBootMsg
(
uasId
,
mlBoot
);
...
...
@@ -138,11 +142,9 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
case
MAVLINK_MSG_ID_PID
:
//182
memset
(
&
mlSinglePid
,
0
,
sizeof
(
mavlink_pid_t
));
mavlink_msg_pid_decode
(
&
message
,
&
mlSinglePid
);
qDebug
()
<<
"
\n
SLUGS RECEIVED PID Message = "
<<
mlSinglePid
.
idx
;
//
qDebug() << "\nSLUGS RECEIVED PID Message = "<<mlSinglePid.idx;
emit
slugsPidValues
(
uasId
,
mlSinglePid
);
break
;
case
MAVLINK_MSG_ID_SLUGS_ACTION
:
//183
...
...
@@ -179,7 +181,6 @@ void SlugsMAV::emitSignals (void){
emit
remoteControlChannelScaledChanged
(
1
,(
mlPilotConsoleData
.
dla
-
1000.0
f
)
/
1000.0
f
);
emit
remoteControlChannelScaledChanged
(
2
,(
mlPilotConsoleData
.
dr
-
1000.0
f
)
/
1000.0
f
);
emit
remoteControlChannelScaledChanged
(
3
,(
mlPilotConsoleData
.
dra
-
1000.0
f
)
/
1000.0
f
);
emit
remoteControlChannelScaledChanged
(
0
,(
mlPilotConsoleData
.
dt
-
1000.0
f
)
/
1000.0
f
);
emit
slugsPWM
(
uasId
,
mlPwmCommands
);
break
;
...
...
@@ -222,7 +223,7 @@ void SlugsMAV::emitSignals (void){
#ifdef MAVLINK_ENABLED_SLUGS
void
SlugsMAV
::
emitGpsSignals
(
void
){
qDebug
()
<<
"After Emit GPS Signal"
<<
mlGpsData
.
fix_type
;
//
qDebug()<<"After Emit GPS Signal"<<mlGpsData.fix_type;
//ToDo Uncomment if. it was comment only to test
...
...
@@ -250,9 +251,15 @@ void SlugsMAV::emitGpsSignals (void){
void
SlugsMAV
::
emitPidSignal
()
{
qDebug
()
<<
"
\n
SLUGS RECEIVED PID Message"
;
//
qDebug() << "\nSLUGS RECEIVED PID Message";
emit
slugsPidValues
(
uasId
,
mlSinglePid
);
}
mavlink_pwm_commands_t
*
SlugsMAV
::
getPwmCommands
()
{
return
&
mlPwmCommands
;
}
#endif // MAVLINK_ENABLED_SLUGS
src/uas/SlugsMAV.h
View file @
c8fe3480
...
...
@@ -36,12 +36,15 @@ class SlugsMAV : public UAS
public:
SlugsMAV
(
MAVLinkProtocol
*
mavlink
,
int
id
=
0
);
public
slots
:
/** @brief Receive a MAVLink message from this MAV */
void
receiveMessage
(
LinkInterface
*
link
,
mavlink_message_t
message
);
void
emitSignals
(
void
);
//mavlink_pwm_commands_t* getPwmCommands();
signals:
...
...
@@ -70,6 +73,8 @@ signals:
#endif
protected:
...
...
src/uas/UAS.h
View file @
c8fe3480
...
...
@@ -92,7 +92,7 @@ public:
friend
class
UASWaypointManager
;
protected:
protected:
//COMMENTS FOR TEST UNIT
int
uasId
;
///< Unique system ID
unsigned
char
type
;
///< UAS type (from type enum)
quint64
startTime
;
///< The time the UAS was switched on
...
...
src/uas/UASWaypointManager.cc
View file @
c8fe3480
...
...
@@ -31,6 +31,7 @@ This file is part of the QGROUNDCONTROL project
#include "UASWaypointManager.h"
#include "UAS.h"
#include "mavlink_types.h"
#define PROTOCOL_TIMEOUT_MS 2000 ///< maximum time to wait for pending messages until timeout
#define PROTOCOL_DELAY_MS 40 ///< minimum delay between sent messages
...
...
@@ -604,22 +605,26 @@ void UASWaypointManager::sendWaypointRequest(quint16 seq)
void
UASWaypointManager
::
sendWaypoint
(
quint16
seq
)
{
mavlink_message_t
message
;
qDebug
()
<<
" WP Buffer count: "
<<
waypoint_buffer
.
count
();
if
(
seq
<
waypoint_buffer
.
count
())
{
mavlink_waypoint_t
*
wp
;
wp
=
waypoint_buffer
.
at
(
seq
);
wp
->
target_system
=
uas
.
getUASID
();
wp
->
target_component
=
MAV_COMP_ID_WAYPOINTPLANNER
;
emit
updateStatusString
(
QString
(
"Sending waypoint ID %1 of %2 total"
).
arg
(
wp
->
seq
).
arg
(
current_count
));
qDebug
()
<<
"sent waypoint ("
<<
wp
->
seq
<<
") to ID "
<<
wp
->
target_system
<<
" WP Buffer count: "
<<
waypoint_buffer
.
count
();
mavlink_msg_waypoint_encode
(
uas
.
mavlink
->
getSystemId
(),
uas
.
mavlink
->
getComponentId
(),
&
message
,
wp
);
uas
.
sendMessage
(
message
);
MG
::
SLEEP
::
usleep
(
PROTOCOL_DELAY_MS
*
1000
);
qDebug
()
<<
"sent waypoint ("
<<
wp
->
seq
<<
") to ID "
<<
wp
->
target_system
;
}
}
...
...
src/ui/MainWindow.cc
View file @
c8fe3480
...
...
@@ -172,11 +172,6 @@ MainWindow::MainWindow(QWidget *parent):
// Create actions
connectCommonActions
();
// Add option for custom widgets
connect
(
ui
.
actionNewCustomWidget
,
SIGNAL
(
triggered
()),
this
,
SLOT
(
createCustomWidget
()));
// Allow to mute audio
ui
.
actionMuteAudioOutput
->
setChecked
(
GAudioOutput
::
instance
()
->
isMuted
());
connect
(
ui
.
actionMuteAudioOutput
,
SIGNAL
(
triggered
(
bool
)),
GAudioOutput
::
instance
(),
SLOT
(
mute
(
bool
)));
// Set dock options
setDockOptions
(
AnimatedDocks
|
AllowTabbedDocks
|
AllowNestedDocks
);
...
...
@@ -344,6 +339,21 @@ void MainWindow::buildCommonWidgets()
addToCentralWidgetsMenu
(
protocolWidget
,
"Mavlink Generator"
,
SLOT
(
showCentralWidget
()),
CENTRAL_PROTOCOL
);
}
//TODO temporaly debug
if
(
!
slugsHilSimWidget
)
{
slugsHilSimWidget
=
new
QDockWidget
(
tr
(
"Slugs Hil Sim"
),
this
);
slugsHilSimWidget
->
setWidget
(
new
SlugsHilSim
(
this
));
addToToolsMenu
(
slugsHilSimWidget
,
tr
(
"HIL Sim Configuration"
),
SLOT
(
showToolWidget
()),
MENU_SLUGS_HIL
,
Qt
::
LeftDockWidgetArea
);
}
//TODO temporaly debug
if
(
!
slugsCamControlWidget
)
{
slugsCamControlWidget
=
new
QDockWidget
(
tr
(
"Slugs Video Camera Control"
),
this
);
slugsCamControlWidget
->
setWidget
(
new
SlugsVideoCamControl
(
this
));
addToToolsMenu
(
slugsCamControlWidget
,
tr
(
"Camera Control"
),
SLOT
(
showToolWidget
()),
MENU_SLUGS_CAMERA
,
Qt
::
BottomDockWidgetArea
);
}
if
(
!
dataplotWidget
)
{
dataplotWidget
=
new
QGCDataPlot2D
(
this
);
...
...
@@ -695,8 +705,8 @@ void MainWindow::addToToolsMenu ( QWidget* widget,
// connect the action
connect
(
tempAction
,
SIGNAL
(
triggered
()),
this
,
slotName
);
connect
(
qobject_cast
<
QDockWidget
*>
(
dockWidgets
[
tool
]),
SIGNAL
(
visibilityChanged
(
bool
)),
this
,
SLOT
(
updateVisibilitySettings
(
bool
)));
//
connect(qobject_cast <QDockWidget *>(dockWidgets[tool]),
//
SIGNAL(visibilityChanged(bool)), this, SLOT(updateVisibilitySettings(bool)));
connect
(
qobject_cast
<
QDockWidget
*>
(
dockWidgets
[
tool
]),
SIGNAL
(
dockLocationChanged
(
Qt
::
DockWidgetArea
)),
this
,
SLOT
(
updateLocationSettings
(
Qt
::
DockWidgetArea
)));
...
...
@@ -720,6 +730,7 @@ void MainWindow::showToolWidget()
removeDockWidget
(
qobject_cast
<
QDockWidget
*>
(
dockWidgets
[
tool
]));
}
}
}
...
...
@@ -798,11 +809,9 @@ void MainWindow::updateVisibilitySettings (bool vis)
if
(
temp
)
{
QHashIterator
<
int
,
QWidget
*>
i
(
dockWidgets
);
while
(
i
.
hasNext
())
{
while
(
i
.
hasNext
())
{
i
.
next
();
if
((
static_cast
<
QDockWidget
*>
(
dockWidgets
[
i
.
key
()]))
==
temp
)
{
if
((
static_cast
<
QDockWidget
*>
(
dockWidgets
[
i
.
key
()]))
==
temp
)
{
QString
chKey
=
buildMenuKey
(
SUB_SECTION_CHECKED
,
static_cast
<
TOOLS_WIDGET_NAMES
>
(
i
.
key
()),
currentView
);
settings
.
setValue
(
chKey
,
vis
);
toolsMenuActions
[
i
.
key
()]
->
setChecked
(
vis
);
...
...
@@ -851,6 +860,12 @@ void MainWindow::connectCommonWidgets()
// it notifies that a waypoint global goes to do create and a map graphic too
connect
(
waypointsDockWidget
->
widget
(),
SIGNAL
(
createWaypointAtMap
(
QPointF
)),
mapWidget
,
SLOT
(
createWaypointGraphAtMap
(
QPointF
)));
}
//TODO temporaly debug
if
(
slugsHilSimWidget
&&
slugsHilSimWidget
->
widget
()){
connect
(
UASManager
::
instance
(),
SIGNAL
(
activeUASSet
(
UASInterface
*
)),
slugsHilSimWidget
->
widget
(),
SLOT
(
activeUasSet
(
UASInterface
*
)));
}
}
void
MainWindow
::
createCustomWidget
()
...
...
@@ -1363,10 +1378,10 @@ void MainWindow::UASCreated(UASInterface* uas)
// Connect Pixhawk Actions
connectPxActions
();
}
break
;
loadOperatorView
();
}
// Change the view only if this is the first UAS
...
...
@@ -1568,7 +1583,7 @@ void MainWindow::presentView()
if
(
settings
.
value
(
buildMenuKey
(
SUB_SECTION_CHECKED
,
MENU_HUD
,
currentView
)).
toBool
())
{
addDockWidget
(
static_cast
<
Qt
::
DockWidgetArea
>
(
settings
.
value
(
buildMenuKey
(
SUB_SECTION_LOCATION
,
MENU_HUD
,
currentView
)).
toInt
()),
hsi
DockWidget
);
headUp
DockWidget
);
headUpDockWidget
->
show
();
}
else
...
...
src/ui/MapWidget.cc
View file @
c8fe3480
...
...
@@ -813,7 +813,8 @@ void MapWidget::changeGlobalWaypointPositionBySpinBox(int index, float lat, floa
void
MapWidget
::
updateCameraPosition
(
double
radio
,
double
bearing
,
QString
dir
)
{
// FIXME Mariano
Q_UNUSED
(
bearing
);
Q_UNUSED
(
dir
);
//camPoints.clear();
QPointF
currentPos
=
mc
->
currentCoordinate
();
// QPointF actualPos = getPointxBearing_Range(currentPos.y(),currentPos.x(),bearing,distance);
...
...
src/ui/SlugsDataSensorView.cc
View file @
c8fe3480
...
...
@@ -28,7 +28,7 @@ SlugsDataSensorView::~SlugsDataSensorView()
void
SlugsDataSensorView
::
addUAS
(
UASInterface
*
uas
)
{
SlugsMAV
*
slugsMav
=
dynamic
_cast
<
SlugsMAV
*>
(
uas
);
SlugsMAV
*
slugsMav
=
qobject
_cast
<
SlugsMAV
*>
(
uas
);
if
(
slugsMav
!=
NULL
)
{
...
...
@@ -104,7 +104,7 @@ void SlugsDataSensorView::slugsGlobalPositionChanged(UASInterface *uas,
ui
->
m_GpsLongitude
->
setText
(
QString
::
number
(
lon
));
ui
->
m_GpsHeight
->
setText
(
QString
::
number
(
alt
));
qDebug
()
<<
"GPS Position = "
<<
lat
<<
" - "
<<
lon
<<
" - "
<<
alt
;
//
qDebug()<<"GPS Position = "<<lat<<" - "<<lon<<" - "<<alt;
}
...
...
@@ -154,7 +154,7 @@ void SlugsDataSensorView::slugAttitudeChanged(UASInterface* uas,
ui
->
m_Pitch
->
setPlainText
(
QString
::
number
(
slugpitch
));
ui
->
m_Yaw
->
setPlainText
(
QString
::
number
(
slugyaw
));
qDebug
()
<<
"Attitude change = "
<<
slugroll
<<
" - "
<<
slugpitch
<<
" - "
<<
slugyaw
;
//
qDebug()<<"Attitude change = "<<slugroll<<" - "<<slugpitch<<" - "<<slugyaw;
}
...
...
src/ui/SlugsDataSensorView.ui
View file @
c8fe3480
...
...
@@ -7,7 +7,7 @@
<x>
0
</x>
<y>
0
</y>
<width>
399
</width>
<height>
604
</height>
<height>
598
</height>
</rect>
</property>
<property
name=
"sizePolicy"
>
...
...
@@ -31,18 +31,18 @@
<property
name=
"windowTitle"
>
<string>
Form
</string>
</property>
<layout
class=
"QGridLayout"
name=
"gridLayout_2
3
"
>
<layout
class=
"QGridLayout"
name=
"gridLayout_2
5
"
>
<item
row=
"0"
column=
"0"
>
<widget
class=
"QTabWidget"
name=
"SlugsSensorView_tabWidget"
>
<property
name=
"currentIndex"
>
<number>
1
</number>
<number>
0
</number>
</property>
<widget
class=
"QWidget"
name=
"tab"
>
<attribute
name=
"title"
>
<string>
Tab 1
</string>
</attribute>
<layout
class=
"Q
VBoxLayout"
name=
"verticalLayout_10
"
>
<item>
<layout
class=
"Q
GridLayout"
name=
"gridLayout_24
"
>
<item
row=
"0"
column=
"0"
>
<layout
class=
"QVBoxLayout"
name=
"verticalLayout_9"
>
<item>
<widget
class=
"QGroupBox"
name=
"position_groupBox"
>
...
...
@@ -2904,10 +2904,8 @@
<attribute
name=
"title"
>
<string>
Tab 3
</string>
</attribute>
<layout
class=
"QVBoxLayout"
name=
"verticalLayout_16"
>
<item>
<layout
class=
"QHBoxLayout"
name=
"horizontalLayout_4"
>
<item>
<layout
class=
"QGridLayout"
name=
"gridLayout_23"
>
<item
row=
"0"
column=
"0"
>
<widget
class=
"QGroupBox"
name=
"sensorBiases_groupBox_2"
>
<property
name=
"minimumSize"
>
<size>
...
...
@@ -2924,8 +2922,8 @@
<property
name=
"title"
>
<string>
CPU Load
</string>
</property>
<layout
class=
"QVBoxLayout"
name=
"verticalLayout_12
"
>
<item
>
<layout
class=
"QGridLayout"
name=
"gridLayout_20
"
>
<item
row=
"0"
column=
"0"
>
<layout
class=
"QGridLayout"
name=
"gridLayout_10"
>
<item
row=
"0"
column=
"0"
>
<widget
class=
"QLabel"
name=
"label_55"
>
...
...
@@ -3109,7 +3107,7 @@
</layout>
</widget>
</item>
<item
>
<item
row=
"0"
column=
"1"
>
<widget
class=
"QGroupBox"
name=
"sensorBiases_groupBox_3"
>
<property
name=
"minimumSize"
>
<size>
...
...
@@ -3126,8 +3124,8 @@
<property
name=
"title"
>
<string>
Air Data
</string>
</property>
<layout
class=
"QVBoxLayout"
name=
"verticalLayout_13
"
>
<item
>
<layout
class=
"QGridLayout"
name=
"gridLayout_21
"
>
<item
row=
"0"
column=
"0"
>
<layout
class=
"QGridLayout"
name=
"gridLayout_11"
>
<item
row=
"0"
column=
"0"
>
<widget
class=
"QLabel"
name=
"label_56"
>
...
...
@@ -3311,9 +3309,7 @@
</layout>
</widget>
</item>
</layout>
</item>
<item>
<item
row=
"1"
column=
"0"
colspan=
"2"
>
<widget
class=
"QGroupBox"
name=
"sensorBiases_groupBox_4"
>
<property
name=
"minimumSize"
>
<size>
...
...
@@ -3330,8 +3326,6 @@
<property
name=
"title"
>
<string>
Filtered Data
</string>
</property>
<layout
class=
"QVBoxLayout"
name=
"verticalLayout_14"
>
<item>
<layout
class=
"QGridLayout"
name=
"gridLayout_15"
>
<item
row=
"0"
column=
"0"
>
<layout
class=
"QGridLayout"
name=
"gridLayout_12"
>
...
...
@@ -3520,7 +3514,7 @@
</item>
</layout>
</item>
<item
row=
"0"
column=
"1"
rowspan=
"2
"
>
<item
row=
"0"
column=
"1
"
>
<layout
class=
"QGridLayout"
name=
"gridLayout_14"
>
<item
row=
"0"
column=
"0"
>
<widget
class=
"QLabel"
name=
"label_25"
>
...
...
@@ -3701,7 +3695,7 @@
</item>
</layout>
</item>
<item
row=
"1"
column=
"0"
rowspan=
"2
"
>
<item
row=
"1"
column=
"0
"
>
<layout
class=
"QGridLayout"
name=
"gridLayout_13"
>
<item
row=
"0"
column=
"0"
>
<widget
class=
"QLabel"
name=
"label_66"
>
...
...
@@ -3882,7 +3876,7 @@
</item>
</layout>
</item>
<item
row=
"2
"
column=
"1"
>
<item
row=
"1
"
column=
"1"
>
<spacer
name=
"verticalSpacer_3"
>
<property
name=
"orientation"
>
<enum>
Qt::Vertical
</enum>
...
...
@@ -3890,17 +3884,15 @@
<property
name=
"sizeHint"
stdset=
"0"
>
<size>
<width>
20
</width>
<height>
40
</height>
<height>
84
</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item>
<item
row=
"2"
column=
"0"
colspan=
"2"
>
<widget
class=
"QGroupBox"
name=
"sensorBiases_groupBox_5"
>
<property
name=
"minimumSize"
>
<size>
...
...
@@ -3917,8 +3909,6 @@
<property
name=
"title"
>
<string>
Raw Data
</string>
</property>
<layout
class=
"QVBoxLayout"
name=
"verticalLayout_15"
>
<item>
<layout
class=
"QGridLayout"
name=
"gridLayout_16"
>
<item
row=
"0"
column=
"0"
>
<layout
class=
"QGridLayout"
name=
"gridLayout_17"
>
...
...
@@ -4101,7 +4091,7 @@
</item>
</layout>
</item>
<item
row=
"0"
column=
"1"
rowspan=
"2
"
>
<item
row=
"0"
column=
"1
"
>
<layout
class=
"QGridLayout"
name=
"gridLayout_18"
>
<item
row=
"0"
column=
"0"
>
<widget
class=
"QLabel"
name=
"label_71"
>
...
...
@@ -4282,7 +4272,7 @@
</item>
</layout>
</item>
<item
row=
"1"
column=
"0"
rowspan=
"2
"
>
<item
row=
"1"
column=
"0
"
>
<layout
class=
"QGridLayout"
name=
"gridLayout_19"
>
<item
row=
"0"
column=
"0"
>
<widget
class=
"QLabel"
name=
"label_74"
>
...
...
@@ -4463,7 +4453,7 @@
</item>
</layout>
</item>
<item
row=
"2
"
column=
"1"
>
<item
row=
"1
"
column=
"1"
>
<spacer
name=
"verticalSpacer_4"
>
<property
name=
"orientation"
>
<enum>
Qt::Vertical
</enum>
...
...
@@ -4471,14 +4461,12 @@
<property
name=
"sizeHint"
stdset=
"0"
>
<size>
<width>
20
</width>
<height>
40
</height>
<height>
84
</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
</layout>
</widget>
</item>
</layout>
...
...
src/ui/SlugsHilSim.cc
View file @
c8fe3480
...
...
@@ -25,12 +25,13 @@ This file is part of the QGROUNDCONTROL project
* @file
* @brief Configuration Window for Slugs' HIL Simulator
* @author Mariano Lizarraga <malife@gmail.com>
* @author Alejandro Molina <am.alex09@gmail.com>
*/
#include "SlugsHilSim.h"
#include "ui_SlugsHilSim.h"
#include "LinkManager.h"
SlugsHilSim
::
SlugsHilSim
(
QWidget
*
parent
)
:
QWidget
(
parent
),
...
...
@@ -49,6 +50,18 @@ SlugsHilSim::SlugsHilSim(QWidget *parent) :
connect
(
rxSocket
,
SIGNAL
(
readyRead
()),
this
,
SLOT
(
readDatagram
()));
linksAvailable
.
clear
();
memset
(
&
tmpAirData
,
0
,
sizeof
(
mavlink_air_data_t
));
memset
(
&
tmpAttitudeData
,
0
,
sizeof
(
mavlink_attitude_t
));
memset
(
&
tmpGpsData
,
0
,
sizeof
(
mavlink_gps_raw_t
));
memset
(
&
tmpGpsTime
,
0
,
sizeof
(
mavlink_gps_date_time_t
));
memset
(
&
tmpLocalPositionData
,
0
,
sizeof
(
mavlink_sensor_bias_t
));
memset
(
&
tmpRawImuData
,
0
,
sizeof
(
mavlink_raw_imu_t
));
foreach
(
LinkInterface
*
link
,
LinkManager
::
instance
()
->
getLinks
())
{
addToCombo
(
link
);
}
}
SlugsHilSim
::~
SlugsHilSim
()
...
...
@@ -59,8 +72,8 @@ SlugsHilSim::~SlugsHilSim()
void
SlugsHilSim
::
addToCombo
(
LinkInterface
*
theLink
){
ui
->
cb_mavlinkLinks
->
addItem
(
theLink
->
getName
());
linksAvailable
.
insert
(
ui
->
cb_mavlinkLinks
->
count
(),
theLink
);
ui
->
cb_mavlinkLinks
->
addItem
(
theLink
->
getName
());
if
(
hilLink
==
NULL
){
hilLink
=
theLink
;
...
...
@@ -121,6 +134,10 @@ void SlugsHilSim::readDatagram(void){
if
(
datagram
.
size
()
==
113
)
{
processHilDatagram
(
&
datagram
);
sendMessageToSlugs
();
commandDatagramToSimulink
();
}
ui
->
ed_count
->
setText
(
QString
::
number
(
count
++
));
...
...
@@ -141,12 +158,6 @@ void SlugsHilSim::processHilDatagram(const QByteArray* datagram)
#ifdef MAVLINK_ENABLED_SLUGS
unsigned
char
i
=
0
;
mavlink_message_t
msg
;
// GPS
mavlink_gps_raw_t
tmpGpsRaw
;
mavlink_gps_date_time_t
tmpGpsTime
;
tmpGpsTime
.
year
=
datagram
->
at
(
i
++
);
tmpGpsTime
.
month
=
datagram
->
at
(
i
++
);
...
...
@@ -155,31 +166,64 @@ void SlugsHilSim::processHilDatagram(const QByteArray* datagram)
tmpGpsTime
.
min
=
datagram
->
at
(
i
++
);
tmpGpsTime
.
sec
=
datagram
->
at
(
i
++
);
tmpGps
Raw
.
lat
=
getFloatFromDatagram
(
datagram
,
&
i
);
tmpGps
Raw
.
lon
=
getFloatFromDatagram
(
datagram
,
&
i
);
tmpGps
Raw
.
alt
=
getFloatFromDatagram
(
datagram
,
&
i
);
tmpGps
Data
.
lat
=
getFloatFromDatagram
(
datagram
,
&
i
);
tmpGps
Data
.
lon
=
getFloatFromDatagram
(
datagram
,
&
i
);
tmpGps
Data
.
alt
=
getFloatFromDatagram
(
datagram
,
&
i
);
tmpGpsRaw
.
hdg
=
getUint16FromDatagram
(
datagram
,
&
i
);
tmpGpsRaw
.
v
=
getUint16FromDatagram
(
datagram
,
&
i
);
tmpGpsRaw
.
eph
=
getUint16FromDatagram
(
datagram
,
&
i
);
tmpGpsData
.
hdg
=
getUint16FromDatagram
(
datagram
,
&
i
);
tmpGpsData
.
v
=
getUint16FromDatagram
(
datagram
,
&
i
);
tmpGpsRaw
.
fix_type
=
datagram
->
at
(
i
++
);
tmpGpsData
.
eph
=
getUint16FromDatagram
(
datagram
,
&
i
);
tmpGpsData
.
fix_type
=
datagram
->
at
(
i
++
);
tmpGpsTime
.
visSat
=
datagram
->
at
(
i
++
);
i
++
;
mavlink_msg_gps_date_time_encode
(
MG
::
SYSTEM
::
ID
,
MG
::
SYSTEM
::
COMPID
,
&
msg
,
&
tmpGpsTime
);
activeUas
->
sendMessage
(
hilLink
,
msg
);
memset
(
&
msg
,
0
,
sizeof
(
mavlink_message_t
));
mavlink_msg_gps_raw_encode
(
MG
::
SYSTEM
::
ID
,
MG
::
SYSTEM
::
COMPID
,
&
msg
,
&
tmpGpsRaw
);
activeUas
->
sendMessage
(
hilLink
,
msg
);
tmpAirData
.
dynamicPressure
=
getFloatFromDatagram
(
datagram
,
&
i
);
tmpAirData
.
staticPressure
=
getFloatFromDatagram
(
datagram
,
&
i
);
tmpAirData
.
temperature
=
getUint16FromDatagram
(
datagram
,
&
i
);
// TODO Salto en el Datagrama
i
=
i
+
8
;
tmpRawImuData
.
xgyro
=
getUint16FromDatagram
(
datagram
,
&
i
);
tmpRawImuData
.
ygyro
=
getUint16FromDatagram
(
datagram
,
&
i
);
tmpRawImuData
.
zgyro
=
getUint16FromDatagram
(
datagram
,
&
i
);
tmpRawImuData
.
xacc
=
getUint16FromDatagram
(
datagram
,
&
i
);
tmpRawImuData
.
yacc
=
getUint16FromDatagram
(
datagram
,
&
i
);
tmpRawImuData
.
zacc
=
getUint16FromDatagram
(
datagram
,
&
i
);
tmpRawImuData
.
xmag
=
getUint16FromDatagram
(
datagram
,
&
i
);
tmpRawImuData
.
ymag
=
getUint16FromDatagram
(
datagram
,
&
i
);
tmpRawImuData
.
zmag
=
getUint16FromDatagram
(
datagram
,
&
i
);
tmpAttitudeData
.
roll
=
getFloatFromDatagram
(
datagram
,
&
i
);
tmpAttitudeData
.
pitch
=
getFloatFromDatagram
(
datagram
,
&
i
);
tmpAttitudeData
.
yaw
=
getFloatFromDatagram
(
datagram
,
&
i
);
tmpAttitudeData
.
rollspeed
=
getFloatFromDatagram
(
datagram
,
&
i
);
tmpAttitudeData
.
pitchspeed
=
getFloatFromDatagram
(
datagram
,
&
i
);
tmpAttitudeData
.
yawspeed
=
getFloatFromDatagram
(
datagram
,
&
i
);
// TODO Crear Paquete SYNC TIME
i
=
i
+
2
;
tmpLocalPositionData
.
x
=
getFloatFromDatagram
(
datagram
,
&
i
);
tmpLocalPositionData
.
y
=
getFloatFromDatagram
(
datagram
,
&
i
);
tmpLocalPositionData
.
z
=
getFloatFromDatagram
(
datagram
,
&
i
);
tmpLocalPositionData
.
vx
=
getFloatFromDatagram
(
datagram
,
&
i
);
tmpLocalPositionData
.
vy
=
getFloatFromDatagram
(
datagram
,
&
i
);
tmpLocalPositionData
.
vz
=
getFloatFromDatagram
(
datagram
,
&
i
);
// TODO: this is legacy of old HIL datagram. Need to remove from Simulink model
i
++
;
ui
->
ed_1
->
setText
(
QString
::
number
(
tmpGpsRaw
.
hdg
));
ui
->
ed_2
->
setText
(
QString
::
number
(
tmpGpsRaw
.
v
));
ui
->
ed_3
->
setText
(
QString
::
number
(
tmpGpsRaw
.
eph
));
ui
->
ed_1
->
setText
(
QString
::
number
(
tmpRawImuData
.
xacc
));
ui
->
ed_2
->
setText
(
QString
::
number
(
tmpRawImuData
.
yacc
));
ui
->
ed_3
->
setText
(
QString
::
number
(
tmpRawImuData
.
zacc
));
ui
->
tbA
->
setText
(
QString
::
number
(
tmpRawImuData
.
xgyro
));
ui
->
tbB
->
setText
(
QString
::
number
(
tmpRawImuData
.
ygyro
));
ui
->
tbC
->
setText
(
QString
::
number
(
tmpRawImuData
.
zgyro
));
#else
Q_UNUSED
(
datagram
);
#endif
...
...
@@ -207,6 +251,97 @@ uint16_t SlugsHilSim::getUint16FromDatagram (const QByteArray* datagram, unsigne
}
void
SlugsHilSim
::
linkSelected
(
int
cbIndex
){
#ifdef MAVLINK_ENABLED_SLUGS
// HIL code to go here...
//hilLink = linksAvailable
// FIXME Mariano
hilLink
=
linksAvailable
.
value
(
cbIndex
);
#endif
}
void
SlugsHilSim
::
sendMessageToSlugs
()
{
mavlink_message_t
msg
;
mavlink_msg_local_position_encode
(
MG
::
SYSTEM
::
ID
,
MG
::
SYSTEM
::
COMPID
,
&
msg
,
&
tmpLocalPositionData
);
activeUas
->
sendMessage
(
hilLink
,
msg
);
memset
(
&
msg
,
0
,
sizeof
(
mavlink_message_t
));
mavlink_msg_attitude_encode
(
MG
::
SYSTEM
::
ID
,
MG
::
SYSTEM
::
COMPID
,
&
msg
,
&
tmpAttitudeData
);
activeUas
->
sendMessage
(
hilLink
,
msg
);
memset
(
&
msg
,
0
,
sizeof
(
mavlink_message_t
));
mavlink_msg_raw_imu_encode
(
MG
::
SYSTEM
::
ID
,
MG
::
SYSTEM
::
COMPID
,
&
msg
,
&
tmpRawImuData
);
activeUas
->
sendMessage
(
hilLink
,
msg
);
memset
(
&
msg
,
0
,
sizeof
(
mavlink_message_t
));
mavlink_msg_air_data_encode
(
MG
::
SYSTEM
::
ID
,
MG
::
SYSTEM
::
COMPID
,
&
msg
,
&
tmpAirData
);
activeUas
->
sendMessage
(
hilLink
,
msg
);
memset
(
&
msg
,
0
,
sizeof
(
mavlink_message_t
));
mavlink_msg_gps_raw_encode
(
MG
::
SYSTEM
::
ID
,
MG
::
SYSTEM
::
COMPID
,
&
msg
,
&
tmpGpsData
);
activeUas
->
sendMessage
(
hilLink
,
msg
);
memset
(
&
msg
,
0
,
sizeof
(
mavlink_message_t
));
mavlink_msg_gps_date_time_encode
(
MG
::
SYSTEM
::
ID
,
MG
::
SYSTEM
::
COMPID
,
&
msg
,
&
tmpGpsTime
);
activeUas
->
sendMessage
(
hilLink
,
msg
);
memset
(
&
msg
,
0
,
sizeof
(
mavlink_message_t
));
}
void
SlugsHilSim
::
commandDatagramToSimulink
()
{
//mavlink_pwm_commands_t* pwdC = (static_cast<SlugsMAV*>(activeUas))->getPwmCommands();
mavlink_pwm_commands_t
*
pwdC
;
if
(
pwdC
!=
NULL
){
}
QByteArray
data
;
data
.
resize
(
22
);
unsigned
char
i
=
0
;
setUInt16ToDatagram
(
data
,
&
i
,
1
);
//pwdC->dt_c);
setUInt16ToDatagram
(
data
,
&
i
,
2
);
//pwdC->dla_c);
setUInt16ToDatagram
(
data
,
&
i
,
3
);
//pwdC->dra_c);
setUInt16ToDatagram
(
data
,
&
i
,
4
);
//pwdC->dr_c);
setUInt16ToDatagram
(
data
,
&
i
,
5
);
//pwdC->dle_c);
setUInt16ToDatagram
(
data
,
&
i
,
6
);
//pwdC->dre_c);
setUInt16ToDatagram
(
data
,
&
i
,
7
);
//pwdC->dlf_c);
setUInt16ToDatagram
(
data
,
&
i
,
8
);
//pwdC->drf_c);
setUInt16ToDatagram
(
data
,
&
i
,
9
);
//pwdC->aux1);
setUInt16ToDatagram
(
data
,
&
i
,
10
);
//pwdC->aux2);
setUInt16ToDatagram
(
data
,
&
i
,
11
);
//value default
txSocket
->
writeDatagram
(
data
,
QHostAddress
::
Broadcast
,
ui
->
ed_txPort
->
text
().
toInt
());
}
void
SlugsHilSim
::
setUInt16ToDatagram
(
QByteArray
&
datagram
,
unsigned
char
*
pos
,
uint16_t
value
)
{
tUint16ToChar
tmpUnion
;
tmpUnion
.
uiData
=
value
;
datagram
[(
*
pos
)
++
]
=
tmpUnion
.
chData
[
0
];
datagram
[(
*
pos
)
++
]
=
tmpUnion
.
chData
[
1
];
}
src/ui/SlugsHilSim.h
View file @
c8fe3480
...
...
@@ -30,14 +30,19 @@ This file is part of the QGROUNDCONTROL project
#ifndef SLUGSHILSIM_H
#define SLUGSHILSIM_H
#include <stdint.h>
#include <QWidget>
#include <QHostAddress>
#include <QUdpSocket>
#include <QMessageBox>
#include <QByteArray>
#include "LinkInterface.h"
#include "UAS.h"
#include <stdint.h>
#include "LinkManager.h"
#include "SlugsMAV.h"
namespace
Ui
{
...
...
@@ -52,6 +57,8 @@ public:
explicit
SlugsHilSim
(
QWidget
*
parent
=
0
);
~
SlugsHilSim
();
protected:
LinkInterface
*
hilLink
;
QHostAddress
*
simulinkIp
;
...
...
@@ -59,6 +66,13 @@ protected:
QUdpSocket
*
rxSocket
;
UAS
*
activeUas
;
mavlink_local_position_t
tmpLocalPositionData
;
mavlink_attitude_t
tmpAttitudeData
;
mavlink_raw_imu_t
tmpRawImuData
;
mavlink_air_data_t
tmpAirData
;
mavlink_gps_raw_t
tmpGpsData
;
mavlink_gps_date_time_t
tmpGpsTime
;
public
slots
:
/**
...
...
@@ -121,11 +135,18 @@ private:
}
tUint16ToChar
;
Ui
::
SlugsHilSim
*
ui
;
QHash
<
int
,
LinkInterface
*>
linksAvailable
;
void
processHilDatagram
(
const
QByteArray
*
datagram
);
float
getFloatFromDatagram
(
const
QByteArray
*
datagram
,
unsigned
char
*
i
);
uint16_t
getUint16FromDatagram
(
const
QByteArray
*
datagram
,
unsigned
char
*
i
);
void
setUInt16ToDatagram
(
QByteArray
&
datagram
,
unsigned
char
*
pos
,
uint16_t
value
);
void
sendMessageToSlugs
();
void
commandDatagramToSimulink
();
};
...
...
src/ui/SlugsHilSim.ui
View file @
c8fe3480
...
...
@@ -6,7 +6,7 @@
<rect>
<x>
0
</x>
<y>
0
</y>
<width>
3
25
</width>
<width>
3
37
</width>
<height>
278
</height>
</rect>
</property>
...
...
@@ -349,6 +349,19 @@
</item>
</layout>
</item>
<item>
<layout
class=
"QHBoxLayout"
name=
"horizontalLayout_6"
>
<item>
<widget
class=
"QLineEdit"
name=
"tbA"
/>
</item>
<item>
<widget
class=
"QLineEdit"
name=
"tbB"
/>
</item>
<item>
<widget
class=
"QLineEdit"
name=
"tbC"
/>
</item>
</layout>
</item>
</layout>
</widget>
<resources/>
...
...
src/ui/SlugsPIDControl.cpp
View file @
c8fe3480
...
...
@@ -23,7 +23,7 @@ SlugsPIDControl::SlugsPIDControl(QWidget *parent) :
setGreenColorStyle
();
refreshTimerGet
=
new
QTimer
(
this
);
refreshTimerGet
->
setInterval
(
100
);
//
2
0 Hz
refreshTimerGet
->
setInterval
(
100
);
//
1
0 Hz
connect
(
refreshTimerGet
,
SIGNAL
(
timeout
()),
this
,
SLOT
(
slugsGetGeneral
()));
...
...
@@ -47,9 +47,9 @@ SlugsPIDControl::SlugsPIDControl(QWidget *parent) :
void
SlugsPIDControl
::
activeUasSet
(
UASInterface
*
uas
)
{
#ifdef MAVLINK_ENABLED_SLUGS
SlugsMAV
*
slugsMav
=
dynamic
_cast
<
SlugsMAV
*>
(
uas
);
SlugsMAV
*
slugsMav
=
qobject
_cast
<
SlugsMAV
*>
(
uas
);
if
(
slugsMav
!=
NULL
)
if
(
slugsMav
)
{
connect
(
slugsMav
,
SIGNAL
(
slugsActionAck
(
int
,
const
mavlink_action_ack_t
&
)),
this
,
SLOT
(
recibeMensaje
(
int
,
mavlink_action_ack_t
)));
connect
(
slugsMav
,
SIGNAL
(
slugsPidValues
(
int
,
mavlink_pid_t
)),
this
,
SLOT
(
receivePidValues
(
int
,
mavlink_pid_t
))
);
...
...
@@ -58,15 +58,14 @@ void SlugsPIDControl::activeUasSet(UASInterface* uas)
connect
(
ui
->
getGeneral_pushButton
,
SIGNAL
(
clicked
()),
this
,
SLOT
(
slugsTimerStartGet
()));
}
#endif // MAVLINK_ENABLED_SLUG
#endif // MAVLINK_ENABLED_SLUG
// Set this UAS as active if it is the first one
if
(
activeUAS
==
0
)
if
(
!
activeUAS
)
{
activeUAS
=
uas
;
systemId
=
activeUAS
->
getUASID
();
connect_editLinesPDIvalues
();
//qDebug()<<"------------------->Active UAS ID: "<<uas->getUASID();
}
}
...
...
@@ -507,6 +506,7 @@ void SlugsPIDControl::get_Pitch2dT_PID()
void
SlugsPIDControl
::
recibeMensaje
(
int
systemId
,
const
mavlink_action_ack_t
&
action
)
{
Q_UNUSED
(
systemId
);
ui
->
recepcion_label
->
setText
(
QString
::
number
(
action
.
action
)
+
":"
+
QString
::
number
(
action
.
result
));
}
...
...
src/ui/SlugsPadCameraControl.cpp
View file @
c8fe3480
...
...
@@ -26,16 +26,14 @@ SlugsPadCameraControl::~SlugsPadCameraControl()
void
SlugsPadCameraControl
::
mouseMoveEvent
(
QMouseEvent
*
event
)
{
Q_UNUSED
(
event
);
//emit mouseMoveCoord(event->x(),event->y());
if
(
dragging
)
{
if
(
abs
(
x1
-
event
->
x
())
>
20
||
abs
(
y1
-
event
->
y
())
>
20
)
{
getDeltaPositionPad
(
event
->
x
(),
event
->
y
());
x1
=
event
->
x
();
y1
=
event
->
y
();
}
// getDeltaPositionPad(event->x(), event->y());
}
...
...
@@ -54,7 +52,7 @@ void SlugsPadCameraControl::mouseReleaseEvent(QMouseEvent *event)
{
dragging
=
false
;
//emit mouseReleaseCoord(event->x(),event->y());
//
getDeltaPositionPad(event->x(), event->y());
getDeltaPositionPad
(
event
->
x
(),
event
->
y
());
xFin
=
event
->
x
();
yFin
=
event
->
y
();
...
...
@@ -105,6 +103,10 @@ void SlugsPadCameraControl::getDeltaPositionPad(int x2, int y2)
double
bearing
=
localMeasures
.
x
();
double
dist
=
getDistPixel
(
y1
,
x1
,
y2
,
x2
);
// this only convert real bearing to frame widget bearing
bearing
=
bearing
+
90
;
if
(
bearing
>=
360
)
bearing
=
bearing
-
360
;
if
(((
bearing
>
330
)
&&
(
bearing
<
360
))
||
((
bearing
>=
0
)
&&
(
bearing
<=
30
)))
...
...
@@ -210,16 +212,9 @@ double SlugsPadCameraControl::getDistPixel(int x1, int y1, int x2, int y2)
// distancia = (float) hipotenusa;
}
/**
* Esta funcin xxxxxxxxx
* @param double lat1 -->
* @param double lon1 -->
* @param double lat2 -->
* @param double lon2 -->
* @param ref double rumbo -->
* @param ref double distancia -->
*/
QPointF
SlugsPadCameraControl
::
ObtenerMarcacionDistanciaPixel
(
double
lon1
,
double
lat1
,
double
lon2
,
double
lat2
)
QPointF
SlugsPadCameraControl
::
ObtenerMarcacionDistanciaPixel
(
double
lon1
,
double
lat1
,
double
lon2
,
double
lat2
)
{
double
cateto_opuesto
,
cateto_adyacente
,
hipotenusa
,
distancia
;
double
marcacion
=
0.0
;
...
...
@@ -257,9 +252,7 @@ QPointF SlugsPadCameraControl::ObtenerMarcacionDistanciaPixel(double lon1, doubl
else
if
((
lat1
==
lat2
)
&&
(
lon1
==
lon2
))
//0
marcacion
=
0.0
;
// this only convert real bearing to frame widget bearing
marcacion
=
marcacion
+
90
;
if
(
marcacion
>=
360
)
marcacion
=
marcacion
-
360
;
return
QPointF
(
marcacion
,
distancia
);
...
...
src/ui/SlugsPadCameraControl.h
View file @
c8fe3480
...
...
@@ -19,12 +19,16 @@ public:
public
slots
:
void
getDeltaPositionPad
(
int
x
,
int
y
);
double
getDistPixel
(
int
x1
,
int
y1
,
int
x2
,
int
y2
);
QPointF
ObtenerMarcacionDistanciaPixel
(
double
lon1
,
double
lat1
,
double
lon2
,
double
lat2
);
QPointF
getPointBy_BearingDistance
(
double
lat1
,
double
lon1
,
double
rumbo
,
double
distancia
);
signals:
void
mouseMoveCoord
(
int
x
,
int
y
);
void
mousePressCoord
(
int
x
,
int
y
);
...
...
src/ui/SlugsVideoCamControl.ui
View file @
c8fe3480
...
...
@@ -22,10 +22,10 @@
<property
name=
"windowTitle"
>
<string>
Form
</string>
</property>
<layout
class=
"QGridLayout"
name=
"gridLayout"
>
<item
row=
"0"
column=
"0"
>
<layout
class=
"QVBoxLayout"
name=
"verticalLayout"
>
<item>
<layout
class=
"QGridLayout"
name=
"gridLayout"
>
<item
row=
"0"
column=
"0"
>
<widget
class=
"QLabel"
name=
"label_x"
>
<property
name=
"mouseTracking"
>
<bool>
true
</bool>
...
...
@@ -35,7 +35,7 @@
</property>
</widget>
</item>
<item>
<item
row=
"0"
column=
"1"
>
<widget
class=
"QLabel"
name=
"label_y"
>
<property
name=
"mouseTracking"
>
<bool>
true
</bool>
...
...
@@ -45,21 +45,17 @@
</property>
</widget>
</item>
<item>
<widget
class=
"Q
Label"
name=
"label_dir
"
>
<item
row=
"1"
column=
"1"
>
<widget
class=
"Q
CheckBox"
name=
"viewCamBordeatMap_checkBox
"
>
<property
name=
"text"
>
<string>
Camera
Pos
</string>
<string>
Camera
at Map
</string>
</property>
</widget>
</item>
</layout>
</item>
<item
row=
"1"
column=
"0"
>
<layout
class=
"QHBoxLayout"
name=
"horizontalLayout"
>
<item>
<widget
class=
"QCheckBox"
name=
"viewCamBordeatMap_checkBox"
>
<widget
class=
"QLabel"
name=
"label_dir"
>
<property
name=
"text"
>
<string>
Camera
at Map
</string>
<string>
Camera
Pos
</string>
</property>
</widget>
</item>
...
...
src/ui/designer/QGCParamSlider.cc
View file @
c8fe3480
#include <QMenu>
#include <QContextMenuEvent>
#include <QSettings>
#include "QGCParamSlider.h"
#include "ui_QGCParamSlider.h"
#include "UASInterface.h"
QGCParamSlider
::
QGCParamSlider
(
QWidget
*
parent
)
:
QGCToolWidgetItem
(
"Slider"
,
parent
),
parameterName
(
""
),
...
...
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