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
58137306
Commit
58137306
authored
Nov 21, 2011
by
LM
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Improving state updates for multiple MAVs, fixed Google Maps and Google Earth
parent
ba97a1e3
Changes
11
Expand all
Hide whitespace changes
Inline
Side-by-side
Showing
11 changed files
with
177 additions
and
106 deletions
+177
-106
earth.html
images/earth.html
+10
-10
MAVLinkSimulationMAV.cc
src/comm/MAVLinkSimulationMAV.cc
+2
-2
QGCFlightGearLink.cc
src/comm/QGCFlightGearLink.cc
+1
-1
UDPLink.cc
src/comm/UDPLink.cc
+1
-1
PxQuadMAV.cc
src/uas/PxQuadMAV.cc
+10
-5
UAS.cc
src/uas/UAS.cc
+16
-8
HDDisplay.cc
src/ui/HDDisplay.cc
+2
-2
QGCMAVLinkInspector.cc
src/ui/QGCMAVLinkInspector.cc
+0
-7
QGCGoogleEarthView.cc
src/ui/map3D/QGCGoogleEarthView.cc
+7
-6
UASView.cc
src/ui/uas/UASView.cc
+125
-64
UASView.h
src/ui/uas/UASView.h
+3
-0
No files found.
images/earth.html
View file @
58137306
...
...
@@ -417,7 +417,9 @@ function createAircraft(id, type, color)
planeOrient
=
ge
.
createOrientation
(
''
);
planeModel
.
setOrientation
(
planeOrient
);
planeLink
.
setHref
(
'
http://qgroundcontrol.org/_media/users/models/sfly-hex.dae
'
);
//planeLink.setHref('http://www.asl.ethz.ch/people/rudink/senseSoarDummy.dae');
//planeLink.setHref('http://qgroundcontrol.org/_media/users/models/sfly-hex.dae');
planeLink
.
setHref
(
'
http://qgroundcontrol.org/_media/users/models/ascent-park-glider.dae
'
);
planeModel
.
setLink
(
planeLink
);
planeModel
.
setAltitudeMode
(
ge
.
ALTITUDE_ABSOLUTE
);
...
...
@@ -578,18 +580,16 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
currAlt
=
trueGroundAlt
+
0.1
;
}
// Interpolate between t-1 and t and set new states
lastLat
=
lastLat
*
0.8
+
currLat
*
0.2
;
lastLon
=
lastLon
*
0.8
+
currLon
*
0.2
;
lastAlt
=
lastAlt
*
0.8
+
currAlt
*
0.2
;
//currFollowHeading = ((yaw/M_PI)+1.0)*360.0;
lastLat
=
lastLat
*
0.5
+
currLat
*
0.5
;
lastLon
=
lastLon
*
0.5
+
currLon
*
0.5
;
lastAlt
=
lastAlt
*
0.5
+
currAlt
*
0.5
;
// FIXME Currently invalid conversion from right-handed z-down to z-up frame
planeOrient
.
setRoll
(((
roll
/
M_PI
))
*
180.0
+
180.0
);
planeOrient
.
setTilt
(((
pitch
/
M_PI
))
*
180.0
+
180.0
);
planeOrient
.
setRoll
(
+
((
roll
/
M_PI
))
*
180.0
);
planeOrient
.
setTilt
(
-
((
pitch
/
M_PI
))
*
180.0
);
planeOrient
.
setHeading
(((
yaw
/
M_PI
))
*
180.0
-
90.0
);
planeModel
.
setOrientation
(
planeOrient
);
currFollowHeading
=
((
yaw
/
M_PI
))
*
180.0
;
currFollowHeading
=
currFollowHeading
*
0.95
+
0.05
*
(((
yaw
/
M_PI
))
*
180.0
)
;
planeLoc
.
setLatitude
(
lastLat
);
planeLoc
.
setLongitude
(
lastLon
);
...
...
src/comm/MAVLinkSimulationMAV.cc
View file @
58137306
...
...
@@ -14,10 +14,10 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy
timer1Hz
(
0
),
latitude
(
lat
),
longitude
(
lon
),
altitude
(
0.0
),
altitude
(
55
0.0
),
x
(
lat
),
y
(
lon
),
z
(
550
),
z
(
altitude
),
roll
(
0.0
),
pitch
(
0.0
),
yaw
(
0.0
),
...
...
src/comm/QGCFlightGearLink.cc
View file @
58137306
...
...
@@ -188,7 +188,7 @@ void QGCFlightGearLink::writeBytes(const char* data, qint64 size)
void
QGCFlightGearLink
::
readBytes
()
{
const
qint64
maxLength
=
65536
;
static
char
data
[
maxLength
];
char
data
[
maxLength
];
QHostAddress
sender
;
quint16
senderPort
;
...
...
src/comm/UDPLink.cc
View file @
58137306
...
...
@@ -210,7 +210,7 @@ void UDPLink::writeBytes(const char* data, qint64 size)
void
UDPLink
::
readBytes
()
{
const
qint64
maxLength
=
65536
;
static
char
data
[
maxLength
];
char
data
[
maxLength
];
QHostAddress
sender
;
quint16
senderPort
;
...
...
src/uas/PxQuadMAV.cc
View file @
58137306
...
...
@@ -42,9 +42,12 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
#ifdef MAVLINK_ENABLED_PIXHAWK
mavlink_message_t
*
msg
=
&
message
;
if
(
message
.
sysid
==
uasId
)
{
switch
(
message
.
msgid
)
{
case
MAVLINK_MSG_ID_RAW_AUX
:
{
if
(
message
.
sysid
==
uasId
)
{
switch
(
message
.
msgid
)
{
case
MAVLINK_MSG_ID_RAW_AUX
:
{
mavlink_raw_aux_t
raw
;
mavlink_msg_raw_aux_decode
(
&
message
,
&
raw
);
quint64
time
=
getUnixTime
(
0
);
...
...
@@ -52,14 +55,16 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit
valueChanged
(
uasId
,
"Temperature"
,
"raw"
,
raw
.
temp
,
time
);
}
break
;
case
MAVLINK_MSG_ID_IMAGE_TRIGGERED
:
{
case
MAVLINK_MSG_ID_IMAGE_TRIGGERED
:
{
// FIXME Kind of a hack to load data from disk
mavlink_image_triggered_t
img
;
mavlink_msg_image_triggered_decode
(
&
message
,
&
img
);
emit
imageStarted
(
img
.
timestamp
);
}
break
;
case
MAVLINK_MSG_ID_PATTERN_DETECTED
:
{
case
MAVLINK_MSG_ID_PATTERN_DETECTED
:
{
mavlink_pattern_detected_t
detected
;
mavlink_msg_pattern_detected_decode
(
&
message
,
&
detected
);
QByteArray
b
;
...
...
src/uas/UAS.cc
View file @
58137306
...
...
@@ -232,6 +232,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
case
MAV_TYPE_QUADROTOR
:
setAirframe
(
UASInterface
::
QGC_AIRFRAME_CHEETAH
);
break
;
case
MAV_TYPE_HEXAROTOR
:
setAirframe
(
UASInterface
::
QGC_AIRFRAME_HEXCOPTER
);
break
;
default:
// Do nothing
break
;
...
...
@@ -338,8 +341,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit
loadChanged
(
this
,
state
.
load
/
10.0
f
);
currentVoltage
=
state
.
voltage_battery
/
1000.0
f
;
lpVoltage
=
filterVoltage
(
currentVoltage
);
...
...
@@ -463,7 +464,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit
globalPositionChanged
(
this
,
latitude
,
longitude
,
altitude
,
time
);
emit
speedChanged
(
this
,
speedX
,
speedY
,
speedZ
,
time
);
// Set internal state
if
(
!
positionLock
)
{
if
(
!
positionLock
)
{
// If position was not locked before, notify positive
GAudioOutput
::
instance
()
->
notifyPositive
();
}
...
...
@@ -483,29 +485,35 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
// quint64 time = getUnixTime(pos.time_usec);
quint64
time
=
getUnixTime
(
pos
.
time_usec
);
if
(
pos
.
fix_type
>
2
)
{
if
(
pos
.
fix_type
>
2
)
{
emit
globalPositionChanged
(
this
,
pos
.
lat
/
(
double
)
1E7
,
pos
.
lon
/
(
double
)
1E7
,
pos
.
alt
/
1000.0
,
time
);
latitude
=
pos
.
lat
/
(
double
)
1E7
;
longitude
=
pos
.
lon
/
(
double
)
1E7
;
altitude
=
pos
.
alt
/
1000.0
;
positionLock
=
true
;
isGlobalPositionKnown
=
true
;
// Check for NaN
int
alt
=
pos
.
alt
;
if
(
alt
!=
alt
)
{
if
(
!
isnan
(
alt
)
&&
!
isinf
(
alt
))
{
alt
=
0
;
emit
textMessageReceived
(
uasId
,
message
.
compid
,
255
,
"GCS ERROR: RECEIVED NaN FOR ALTITUDE"
);
emit
textMessageReceived
(
uasId
,
message
.
compid
,
255
,
"GCS ERROR: RECEIVED NaN
or Inf
FOR ALTITUDE"
);
}
// FIXME REMOVE LATER emit valueChanged(uasId, "altitude", "m", pos.alt/(double)1E3, time);
// Smaller than threshold and not NaN
float
vel
=
pos
.
vel
/
100.0
f
;
if
(
vel
<
1000000
&&
!
isnan
(
vel
)
&&
!
isinf
(
vel
))
{
if
(
vel
<
1000000
&&
!
isnan
(
vel
)
&&
!
isinf
(
vel
))
{
// FIXME REMOVE LATER emit valueChanged(uasId, "speed", "m/s", vel, time);
//qDebug() << "GOT GPS RAW";
// emit speedChanged(this, (double)pos.v, 0.0, 0.0, time);
}
else
{
}
else
{
emit
textMessageReceived
(
uasId
,
message
.
compid
,
255
,
QString
(
"GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s"
).
arg
(
vel
));
}
}
...
...
src/ui/HDDisplay.cc
View file @
58137306
...
...
@@ -186,9 +186,9 @@ void HDDisplay::triggerUpdate()
void
HDDisplay
::
paintEvent
(
QPaintEvent
*
event
)
{
Q_UNUSED
(
event
);
static
quint64
interval
=
0
;
quint64
interval
=
0
;
//qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__;
interval
=
MG
::
TIME
::
getGroundTimeNow
();
interval
=
QGC
::
groundTimeMilliseconds
();
renderOverlay
();
}
...
...
src/ui/QGCMAVLinkInspector.cc
View file @
58137306
...
...
@@ -41,7 +41,6 @@ void QGCMAVLinkInspector::refreshView()
messageName
=
messageName
.
arg
(
messageInfo
[
msg
->
msgid
].
name
).
arg
(
messagesHz
.
value
(
msg
->
msgid
,
0
),
2
,
'f'
,
0
).
arg
(
msg
->
msgid
);
if
(
!
treeWidgetItems
.
contains
(
msg
->
msgid
))
{
QStringList
fields
;
fields
<<
messageName
;
QTreeWidgetItem
*
widget
=
new
QTreeWidgetItem
(
fields
);
...
...
@@ -72,8 +71,6 @@ void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t m
{
Q_UNUSED
(
link
);
// Only overwrite if system filter is set
// int filterValue = ui->systemComboBox()->value().toInt();
// if (filterValue != )
memcpy
(
receivedMessages
+
message
.
msgid
,
&
message
,
sizeof
(
mavlink_message_t
));
float
msgHz
=
0.0
f
;
...
...
@@ -85,14 +82,10 @@ void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t m
{
msgHz
=
1
;
}
//qDebug() << "DIFF:" << receiveTime - lastMessageUpdate.value(message.msgid);
float
newHz
=
0.001
f
*
msgHz
+
0.999
f
*
messagesHz
.
value
(
message
.
msgid
,
1
);
qDebug
()
<<
"HZ"
<<
newHz
;
messagesHz
.
insert
(
message
.
msgid
,
newHz
);
}
//qDebug() << "MSGHZ:" << messagesHz.value(message.msgid, 1000);
lastMessageUpdate
.
insert
(
message
.
msgid
,
receiveTime
);
}
...
...
src/ui/map3D/QGCGoogleEarthView.cc
View file @
58137306
...
...
@@ -284,8 +284,6 @@ void QGCGoogleEarthView::updateGlobalPosition(UASInterface* uas, double lat, dou
{
Q_UNUSED
(
usec
);
javaScript
(
QString
(
"addTrailPosition(%1, %2, %3, %4);"
).
arg
(
uas
->
getUASID
()).
arg
(
lat
,
0
,
'f'
,
22
).
arg
(
lon
,
0
,
'f'
,
22
).
arg
(
alt
,
0
,
'f'
,
22
));
//qDebug() << QString("addTrailPosition(%1, %2, %3, %4);").arg(uas->getUASID()).arg(lat, 0, 'f', 15).arg(lon, 0, 'f', 15).arg(alt, 0, 'f', 15);
}
void
QGCGoogleEarthView
::
clearTrails
()
...
...
@@ -614,7 +612,8 @@ void QGCGoogleEarthView::updateState()
#if (QGC_EVENTLOOP_DEBUG)
qDebug
()
<<
"EVENTLOOP:"
<<
__FILE__
<<
__LINE__
;
#endif
if
(
gEarthInitialized
)
{
if
(
gEarthInitialized
)
{
int
uasId
=
0
;
double
lat
=
47.3769
;
double
lon
=
8.549444
;
...
...
@@ -628,6 +627,8 @@ void QGCGoogleEarthView::updateState()
QList
<
UASInterface
*>
mavs
=
UASManager
::
instance
()
->
getUASList
();
foreach
(
UASInterface
*
currMav
,
mavs
)
{
// Only update if known
if
(
!
currMav
->
globalPositionKnown
())
continue
;
uasId
=
currMav
->
getUASID
();
lat
=
currMav
->
getLatitude
();
lon
=
currMav
->
getLongitude
();
...
...
@@ -640,9 +641,9 @@ void QGCGoogleEarthView::updateState()
javaScript
(
QString
(
"setAircraftPositionAttitude(%1, %2, %3, %4, %6, %7, %8);"
)
.
arg
(
uasId
)
.
arg
(
lat
,
0
,
'f'
,
15
)
.
arg
(
lon
,
0
,
'f'
,
15
)
.
arg
(
alt
,
0
,
'f'
,
15
)
.
arg
(
lat
,
0
,
'f'
,
22
)
.
arg
(
lon
,
0
,
'f'
,
22
)
.
arg
(
alt
,
0
,
'f'
,
22
)
.
arg
(
roll
,
0
,
'f'
,
9
)
.
arg
(
pitch
,
0
,
'f'
,
9
)
.
arg
(
yaw
,
0
,
'f'
,
9
));
...
...
src/ui/uas/UASView.cc
View file @
58137306
This diff is collapsed.
Click to expand it.
src/ui/uas/UASView.h
View file @
58137306
...
...
@@ -117,6 +117,7 @@ protected:
float
alt
;
float
groundDistance
;
bool
localFrame
;
bool
globalFrameKnown
;
QAction
*
removeAction
;
QAction
*
renameAction
;
QAction
*
selectAction
;
...
...
@@ -126,6 +127,8 @@ protected:
static
const
int
updateInterval
=
800
;
static
const
int
errorUpdateInterval
=
200
;
bool
lowPowerModeEnabled
;
///< Low power mode reduces update rates
unsigned
int
generalUpdateCount
;
///< Skip counter for updates
double
filterTime
;
///< Filter time estimate of battery
void
mouseDoubleClickEvent
(
QMouseEvent
*
event
);
...
...
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