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
f8d26a43
Commit
f8d26a43
authored
Feb 20, 2011
by
Mariano Lizarraga
Browse files
Options
Browse Files
Download
Plain Diff
Merge branch 'experimental' of github.com:pixhawk/qgroundcontrol into experimental
parents
8e0714eb
9802d9fa
Changes
10
Hide whitespace changes
Inline
Side-by-side
Showing
10 changed files
with
370 additions
and
119 deletions
+370
-119
earth.html
images/earth.html
+96
-17
UAS.cc
src/uas/UAS.cc
+15
-1
UAS.h
src/uas/UAS.h
+2
-0
UASInterface.h
src/uas/UASInterface.h
+2
-0
UASManager.cc
src/uas/UASManager.cc
+48
-1
UASManager.h
src/uas/UASManager.h
+19
-0
MainWindow.cc
src/ui/MainWindow.cc
+1
-0
QGCGoogleEarthView.cc
src/ui/map3D/QGCGoogleEarthView.cc
+99
-33
QGCGoogleEarthView.h
src/ui/map3D/QGCGoogleEarthView.h
+4
-0
QGCGoogleEarthView.ui
src/ui/map3D/QGCGoogleEarthView.ui
+84
-67
No files found.
images/earth.html
View file @
f8d26a43
...
...
@@ -7,7 +7,7 @@
<!-- QGroundControl -->
<title>
QGroundControl Google Earth View
</title>
<!-- *** Replace the key below below with your own API key, available at http://code.google.com/apis/maps/signup.html *** -->
<
script
type=
"text/javascript"
src=
"https://getfirebug.com/firebug-lite-beta.js"
></script
>
<
!--<script type="text/javascript" src="https://getfirebug.com/firebug-lite-beta.js"></script>--
>
<script
type=
"text/javascript"
src=
"http://www.google.com/jsapi?key=ABQIAAAAwbkbZLyhsmTCWXbTcjbgbRSzHs7K5SvaUdm8ua-Xxy_-2dYwMxQMhnagaawTo7L1FE1-amhuQxIlXw"
></script>
<script
type=
"text/javascript"
>
google
.
load
(
"
earth
"
,
"
1
"
,
{
'
language
'
:
'
en
'
});
...
...
@@ -23,12 +23,17 @@ var lastAlt = 0;
var
currLat
=
47.3769
;
var
currLon
=
8.549444
;
var
currAlt
=
470
;
var
currentCameraLatitude
=
0
;
var
currentCameraLongitude
=
0
;
var
currentCameraAltitude
=
0
;
var
currFollowHeading
=
0.0
;
var
homeLat
=
0
;
var
homeLon
=
0
;
var
homeAlt
=
0
;
var
homeViewRange
=
5
00
;
var
homeViewRange
=
8
00
;
var
homeLocation
=
null
;
var
homeGroundLevel
=
0
;
...
...
@@ -85,6 +90,8 @@ var newWaypointLongitude = 0;
var
newWaypointAltitude
=
0
;
var
newWaypointPending
=
false
;
var
clickMode
=
0
;
var
homePlacemark
=
null
;
function
getGlobal
(
variable
)
...
...
@@ -107,14 +114,38 @@ function setDraggingAllowed(allowed)
draggingAllowed
=
allowed
;
}
function
sampleCurrentPosition
()
{
var
thisView
=
ge
.
getView
().
copyAsLookAt
(
ge
.
ALTITUDE_ABSOLUTE
);
currentCameraLatitude
=
thisView
.
getLatitude
();
currentCameraLongitude
=
thisView
.
getLongitude
();
currentCameraGroundAltitude
=
ge
.
getGlobe
().
getGroundAltitude
(
currentCameraLatitude
,
currentCameraLongitude
);
}
function
enableSetHomeMode
()
{
clickMode
=
1
;
}
function
setLookAtLatLon
(
lat
,
lon
)
{
// Keep the current altitude above ground, just move the position
currView
=
ge
.
getView
().
copyAsLookAt
(
ge
.
ALTITUDE_RELATIVE_TO_GROUND
);
currView
.
setLatitude
(
lat
);
currView
.
setLongitude
(
lon
);
ge
.
getView
().
setAbstractView
(
currView
);
}
function
setNewWaypointPending
(
pending
)
{
newWaypointPending
=
pending
;
document
.
getElementById
(
'
JScript_newWaypointPending
'
).
setAttribute
(
'
value
'
,
pending
);
}
function
setDragWaypointPending
(
pending
)
{
dragWaypointPending
=
pending
;
document
.
getElementById
(
'
JScript_dragWaypointPending
'
).
setAttribute
(
'
value
'
,
pending
);
}
function
isInitialized
()
...
...
@@ -138,16 +169,35 @@ function enableEventListener()
// listen for mousedown on the window (look specifically for point placemarks)
google
.
earth
.
addEventListener
(
ge
.
getWindow
(),
'
mousedown
'
,
function
(
event
)
{
if
(
clickMode
==
1
)
{
// Set home mode
dragWaypointIndex
=
'
HOME
'
;
document
.
getElementById
(
'
JScript_dragWaypointIndex
'
).
setAttribute
(
'
value
'
,
dragWaypointIndex
);
dragWaypointLatitude
=
event
.
getLatitude
();
dragWaypointLongitude
=
event
.
getLongitude
();
dragWaypointAltitude
=
ge
.
getGlobe
().
getGroundAltitude
(
dragWaypointLatitude
,
dragWaypointLongitude
);
dragWaypointPending
=
true
;
document
.
getElementById
(
'
JScript_dragWaypointLatitude
'
).
setAttribute
(
'
value
'
,
dragWaypointLatitude
);
document
.
getElementById
(
'
JScript_dragWaypointLongitude
'
).
setAttribute
(
'
value
'
,
dragWaypointLongitude
);
document
.
getElementById
(
'
JScript_dragWaypointAltitude
'
).
setAttribute
(
'
value
'
,
dragWaypointAltitude
);
document
.
getElementById
(
'
JScript_dragWaypointPending
'
).
setAttribute
(
'
value
'
,
true
);
setGCSHome
(
dragWaypointLatitude
,
dragWaypointLongitude
,
dragWaypointAltitude
);
}
if
(
event
.
getTarget
().
getType
()
==
'
KmlPlacemark
'
&&
event
.
getTarget
().
getGeometry
().
getType
()
==
'
KmlPoint
'
)
{
var
placemark
=
event
.
getTarget
();
event
.
preventDefault
();
if
(
draggingAllowed
)
{
if
(
clickMode
==
0
)
{
dragInfo
=
{
placemark
:
event
.
getTarget
(),
dragged
:
false
};
}
}
}
});
...
...
@@ -155,7 +205,7 @@ google.earth.addEventListener(ge.getWindow(), 'mousedown', function(event)
// listen for mousemove on the globe
google
.
earth
.
addEventListener
(
ge
.
getGlobe
(),
'
mousemove
'
,
function
(
event
)
{
if
(
draggingAllowed
)
if
(
draggingAllowed
&&
(
clickMode
==
0
)
)
{
if
(
dragInfo
)
{
event
.
preventDefault
();
...
...
@@ -164,10 +214,15 @@ google.earth.addEventListener(ge.getGlobe(), 'mousemove', function(event)
point
.
setLongitude
(
event
.
getLongitude
());
dragInfo
.
dragged
=
true
;
dragWaypointIndex
=
dragInfo
.
placemark
.
getDescription
();
document
.
getElementById
(
'
JScript_dragWaypointIndex
'
).
setAttribute
(
'
value
'
,
dragWaypointIndex
);
dragWaypointLatitude
=
event
.
getLatitude
();
dragWaypointLongitude
=
event
.
getLongitude
();
dragWaypointAltitude
=
point
.
getAltitude
();
dragWaypointPending
=
true
;
document
.
getElementById
(
'
JScript_dragWaypointLatitude
'
).
setAttribute
(
'
value
'
,
dragWaypointLatitude
);
document
.
getElementById
(
'
JScript_dragWaypointLongitude
'
).
setAttribute
(
'
value
'
,
dragWaypointLongitude
);
document
.
getElementById
(
'
JScript_dragWaypointAltitude
'
).
setAttribute
(
'
value
'
,
dragWaypointAltitude
);
document
.
getElementById
(
'
JScript_dragWaypointPending
'
).
setAttribute
(
'
value
'
,
true
);
}
}
});
...
...
@@ -175,7 +230,7 @@ google.earth.addEventListener(ge.getGlobe(), 'mousemove', function(event)
// listen for mouseup on the window
google
.
earth
.
addEventListener
(
ge
.
getWindow
(),
'
mouseup
'
,
function
(
event
)
{
if
(
draggingAllowed
)
if
(
draggingAllowed
&&
(
clickMode
==
0
)
)
{
if
(
dragInfo
)
{
if
(
dragInfo
.
dragged
)
...
...
@@ -184,16 +239,22 @@ google.earth.addEventListener(ge.getWindow(), 'mouseup', function(event)
event
.
preventDefault
();
// Get drag end location
dragWaypointIndex
=
dragInfo
.
placemark
.
getDescription
();
document
.
getElementById
(
'
JScript_dragWaypointIndex
'
).
setAttribute
(
'
value
'
,
dragWaypointIndex
);
var
point
=
dragInfo
.
placemark
.
getGeometry
();
dragWaypointLatitude
=
event
.
getLatitude
();
dragWaypointLongitude
=
event
.
getLongitude
();
dragWaypointAltitude
=
point
.
getAltitude
();
dragWaypointPending
=
true
;
document
.
getElementById
(
'
JScript_dragWaypointLatitude
'
).
setAttribute
(
'
value
'
,
dragWaypointLatitude
);
document
.
getElementById
(
'
JScript_dragWaypointLongitude
'
).
setAttribute
(
'
value
'
,
dragWaypointLongitude
);
document
.
getElementById
(
'
JScript_dragWaypointAltitude
'
).
setAttribute
(
'
value
'
,
dragWaypointAltitude
);
document
.
getElementById
(
'
JScript_dragWaypointPending
'
).
setAttribute
(
'
value
'
,
true
);
}
dragInfo
=
null
;
}
}
clickMode
=
0
;
});
// Listen for wp creation request on the globe
...
...
@@ -206,6 +267,10 @@ google.earth.addEventListener(ge.getGlobe(), 'dblclick', function(event){
newWaypointLongitude
=
event
.
getLongitude
();
newWaypointAltitude
=
ge
.
getGlobe
().
getGroundAltitude
(
newWaypointLatitude
,
newWaypointLongitude
);
newWaypointPending
=
true
;
document
.
getElementById
(
'
JScript_newWaypointLatitude
'
).
setAttribute
(
'
value
'
,
newWaypointLatitude
);
document
.
getElementById
(
'
JScript_newWaypointLongitude
'
).
setAttribute
(
'
value
'
,
newWaypointLongitude
);
document
.
getElementById
(
'
JScript_newWaypointAltitude
'
).
setAttribute
(
'
value
'
,
newWaypointAltitude
);
document
.
getElementById
(
'
JScript_newWaypointPending
'
).
setAttribute
(
'
value
'
,
true
);
}
});
}
...
...
@@ -219,6 +284,9 @@ function setCurrAircraft(id)
function
setGCSHome
(
lat
,
lon
,
alt
)
{
// Only update if position actually changed
if
(
lat
!=
homeLat
||
lon
!=
homeLon
||
alt
!=
homeAlt
)
{
homeLat
=
lat
;
homeLon
=
lon
;
homeAlt
=
alt
;
...
...
@@ -249,12 +317,15 @@ function setGCSHome(lat, lon, alt)
}
else
{
var
location
=
ge
.
createPoint
(
''
);
location
.
setLatitude
(
lat
);
location
.
setLongitude
(
lon
);
location
.
setAltitude
(
alt
);
homePlacemark
.
setGeometry
(
location
);
homePlacemark
.
setDescription
(
'
HOME
'
);
var
location
=
ge
.
createPoint
(
''
);
if
(
location
.
getLatitude
()
!=
lat
||
location
.
getLongitude
()
!=
lon
||
location
.
getAltitude
()
!=
alt
)
{
location
.
setLatitude
(
lat
);
location
.
setLongitude
(
lon
);
location
.
setAltitude
(
alt
);
homePlacemark
.
setGeometry
(
location
);
homePlacemark
.
setDescription
(
'
HOME
'
);
}
}
homeGroundLevel
=
ge
.
getGlobe
().
getGroundAltitude
(
lat
,
lon
);
...
...
@@ -262,6 +333,7 @@ function setGCSHome(lat, lon, alt)
{
homeGroundLevel
=
alt
;
}
}
}
function
updateWaypointListLength
(
id
,
len
)
...
...
@@ -447,7 +519,7 @@ function initCallback(object)
ge
.
getLayerRoot
().
enableLayerById
(
ge
.
LAYER_TREES
,
true
);
enableEventListener
();
document
.
getElementById
(
'
JScript_initialized
'
).
setAttribute
(
'
value
'
,
'
true
'
);
initialized
=
true
;
}
...
...
@@ -544,14 +616,8 @@ function setViewMode(mode)
{
lastTilt
=
currView
.
getTilt
();
lastHeading
=
currView
.
getHeading
();
//var lastLat2 = currView.getLatitude();
//var lastLon2 = currView.getLongitude();
//var lastAlt2 = currView.getAltitude();
currView
.
setTilt
(
0
);
currView
.
setHeading
(
0
);
//currView.setLatitude(lastLat2);
//currView.setLongitude(lastLon2);
//currView.setAltitude(lastAlt2);
}
viewMode
=
mode
;
...
...
@@ -602,5 +668,18 @@ function failureCallback(object)
<center>
<div
id=
'map3d'
style=
'margin: 0; spacing: 0; height: 100%; width: 100%'
></div>
</center>
<input
type=
"hidden"
id=
"JScript_initialized"
value=
"false"
/>
<input
type=
"hidden"
id=
"JScript_dragWaypointIndex"
value=
"0"
/>
<input
type=
"hidden"
id=
"JScript_dragWaypointLatitude"
value=
"0"
/>
<input
type=
"hidden"
id=
"JScript_dragWaypointLongitude"
value=
"0"
/>
<input
type=
"hidden"
id=
"JScript_dragWaypointAltitude"
value=
"0"
/>
<input
type=
"hidden"
id=
"JScript_dragWaypointPending"
value=
"false"
/>
<input
type=
"hidden"
id=
"JScript_newWaypointLatitude"
value=
"0"
/>
<input
type=
"hidden"
id=
"JScript_newWaypointLongitude"
value=
"0"
/>
<input
type=
"hidden"
id=
"JScript_newWaypointAltitude"
value=
"0"
/>
<input
type=
"hidden"
id=
"JScript_newWaypointPending"
value=
"false"
/>
<input
type=
"hidden"
id=
"JScript_currentCameraLatitude"
value=
"0"
/>
<input
type=
"hidden"
id=
"JScript_currentCameraLongitude"
value=
"0"
/>
<input
type=
"hidden"
id=
"JScript_currentCameraGroundAltitude"
value=
"0"
/>
</body>
</html>
src/uas/UAS.cc
View file @
f8d26a43
...
...
@@ -1042,6 +1042,20 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
}
}
void
UAS
::
setHomePosition
(
double
lat
,
double
lon
,
double
alt
)
{
// Send new home position to UAS
mavlink_gps_set_global_origin_t
home
;
home
.
target_system
=
uasId
;
home
.
target_component
=
0
;
// ALL components
home
.
latitude
=
lat
*
1E7
;
home
.
longitude
=
lon
*
1E7
;
home
.
altitude
=
alt
*
1000
;
mavlink_message_t
msg
;
mavlink_msg_gps_set_global_origin_encode
(
mavlink
->
getSystemId
(),
mavlink
->
getComponentId
(),
&
msg
,
&
home
);
sendMessage
(
msg
);
}
void
UAS
::
setLocalOriginAtCurrentGPSPosition
()
{
...
...
@@ -1061,7 +1075,7 @@ void UAS::setLocalOriginAtCurrentGPSPosition()
if
(
ret
==
QMessageBox
::
Yes
)
{
mavlink_message_t
msg
;
mavlink_msg_action_pack
(
mavlink
->
getSystemId
(),
mavlink
->
get
System
Id
(),
&
msg
,
this
->
getUASID
(),
0
,
MAV_ACTION_SET_ORIGIN
);
mavlink_msg_action_pack
(
mavlink
->
getSystemId
(),
mavlink
->
get
Component
Id
(),
&
msg
,
this
->
getUASID
(),
0
,
MAV_ACTION_SET_ORIGIN
);
// Send message twice to increase chance that it reaches its goal
sendMessage
(
msg
);
// Wait 5 ms
...
...
src/uas/UAS.h
View file @
f8d26a43
...
...
@@ -309,6 +309,8 @@ public slots:
/** @brief Set world frame origin at current GPS position */
void
setLocalOriginAtCurrentGPSPosition
();
/** @brief Set world frame origin / home position at this GPS position */
void
setHomePosition
(
double
lat
,
double
lon
,
double
alt
);
/** @brief Set local position setpoint */
void
setLocalPositionSetpoint
(
float
x
,
float
y
,
float
z
,
float
yaw
);
/** @brief Add an offset in body frame to the setpoint */
...
...
src/uas/UASInterface.h
View file @
f8d26a43
...
...
@@ -224,6 +224,8 @@ public slots:
//virtual void clearWaypointList() = 0;
/** @brief Set world frame origin at current GPS position */
virtual
void
setLocalOriginAtCurrentGPSPosition
()
=
0
;
/** @brief Set world frame origin / home position at this GPS position */
virtual
void
setHomePosition
(
double
lat
,
double
lon
,
double
alt
)
=
0
;
/** @brief Request all onboard parameters of all components */
virtual
void
requestParameters
()
=
0
;
/** @brief Request one specific onboard parameter */
...
...
src/uas/UASManager.cc
View file @
f8d26a43
...
...
@@ -32,6 +32,7 @@ This file is part of the QGROUNDCONTROL project
#include <QApplication>
#include <QMessageBox>
#include <QTimer>
#include <QSettings>
#include "UAS.h"
#include "UASInterface.h"
#include "UASManager.h"
...
...
@@ -50,19 +51,64 @@ UASManager* UASManager::instance()
return
_instance
;
}
void
UASManager
::
storeSettings
()
{
QSettings
settings
;
settings
.
beginGroup
(
"QGC_UASMANAGER"
);
settings
.
setValue
(
"HOMELAT"
,
homeLat
);
settings
.
setValue
(
"HOMELON"
,
homeLon
);
settings
.
setValue
(
"HOMEALT"
,
homeAlt
);
settings
.
endGroup
();
settings
.
sync
();
}
void
UASManager
::
loadSettings
()
{
QSettings
settings
;
settings
.
sync
();
settings
.
beginGroup
(
"QGC_UASMANAGER"
);
setHomePosition
(
settings
.
value
(
"HOMELAT"
,
homeLat
).
toDouble
(),
settings
.
value
(
"HOMELON"
,
homeLon
).
toDouble
(),
settings
.
value
(
"HOMEALT"
,
homeAlt
).
toDouble
());
settings
.
endGroup
();
}
void
UASManager
::
setHomePosition
(
double
lat
,
double
lon
,
double
alt
)
{
// Checking for NaN and infitiny
if
(
lat
==
lat
&&
lon
==
lon
&&
alt
==
alt
&&
!
std
::
isinf
(
lat
)
&&
!
std
::
isinf
(
lon
)
&&
!
std
::
isinf
(
alt
))
{
bool
changed
=
false
;
if
(
homeLat
!=
lat
)
changed
=
true
;
if
(
homeLon
!=
lon
)
changed
=
true
;
if
(
homeAlt
!=
alt
)
changed
=
true
;
homeLat
=
lat
;
homeLon
=
lon
;
homeAlt
=
alt
;
if
(
changed
)
emit
homePositionChanged
(
homeLat
,
homeLon
,
homeAlt
);
}
}
/**
* @brief Private singleton constructor
*
* This class implements the singleton design pattern and has therefore only a private constructor.
**/
UASManager
::
UASManager
()
:
activeUAS
(
NULL
)
activeUAS
(
NULL
),
homeLat
(
47.3769
),
homeLon
(
8.549444
),
homeAlt
(
470.0
)
{
start
(
QThread
::
LowPriority
);
loadSettings
();
}
UASManager
::~
UASManager
()
{
storeSettings
();
// Delete all systems
foreach
(
UASInterface
*
mav
,
systems
)
{
...
...
@@ -99,6 +145,7 @@ void UASManager::addUAS(UASInterface* uas)
{
systems
.
append
(
uas
);
connect
(
uas
,
SIGNAL
(
destroyed
(
QObject
*
)),
this
,
SLOT
(
removeUAS
(
QObject
*
)));
connect
(
this
,
SIGNAL
(
homePositionChanged
(
double
,
double
,
double
)),
uas
,
SLOT
(
setHomePosition
(
double
,
double
,
double
)));
emit
UASCreated
(
uas
);
}
...
...
src/uas/UASManager.h
View file @
f8d26a43
...
...
@@ -70,6 +70,12 @@ public:
UASInterface
*
getUASForId
(
int
id
);
QList
<
UASInterface
*>
getUASList
();
/** @brief Get home position latitude */
double
getHomeLatitude
()
const
{
return
homeLat
;
}
/** @brief Get home position longitude */
double
getHomeLongitude
()
const
{
return
homeLon
;
}
/** @brief Get home position altitude */
double
getHomeAltitude
()
const
{
return
homeAlt
;
}
public
slots
:
...
...
@@ -162,12 +168,23 @@ public slots:
/** @brief Shut down the onboard operating system down */
bool
shutdownActiveUAS
();
/** @brief Set the current home position */
void
setHomePosition
(
double
lat
,
double
lon
,
double
alt
);
/** @brief Load settings */
void
loadSettings
();
/** @brief Store settings */
void
storeSettings
();
protected:
UASManager
();
QList
<
UASInterface
*>
systems
;
UASInterface
*
activeUAS
;
QMutex
activeUASMutex
;
double
homeLat
;
double
homeLon
;
double
homeAlt
;
signals:
void
UASCreated
(
UASInterface
*
UAS
);
...
...
@@ -181,6 +198,8 @@ signals:
void
activeUASStatusChanged
(
UASInterface
*
UAS
,
bool
active
);
/** @brief The UAS currently under main operator control changed */
void
activeUASStatusChanged
(
int
systemId
,
bool
active
);
/** @brief Current home position changed */
void
homePositionChanged
(
double
lat
,
double
lon
,
double
alt
);
};
...
...
src/ui/MainWindow.cc
View file @
f8d26a43
...
...
@@ -921,6 +921,7 @@ void MainWindow::closeEvent(QCloseEvent *event)
storeSettings
();
aboutToCloseFlag
=
true
;
mavlink
->
storeSettings
();
UASManager
::
instance
()
->
storeSettings
();
QMainWindow
::
closeEvent
(
event
);
}
...
...
src/ui/map3D/QGCGoogleEarthView.cc
View file @
f8d26a43
...
...
@@ -2,6 +2,7 @@
#include <QDir>
#include <QShowEvent>
#include <QSettings>
#include <QInputDialog>
#include <QDebug>
#include <QFile>
...
...
@@ -19,6 +20,8 @@
#include <QAxObject>
#include <QUuid>
#include <mshtml.h>
#include <comdef.h>
#include <qaxtypes.h>
#endif
#include "QGC.h"
...
...
@@ -86,6 +89,8 @@ QGCGoogleEarthView::QGCGoogleEarthView(QWidget *parent) :
connect
(
ui
->
clearTrailsButton
,
SIGNAL
(
clicked
()),
this
,
SLOT
(
clearTrails
()));
connect
(
ui
->
atmosphereCheckBox
,
SIGNAL
(
clicked
(
bool
)),
this
,
SLOT
(
enableAtmosphere
(
bool
)));
connect
(
ui
->
daylightCheckBox
,
SIGNAL
(
clicked
(
bool
)),
this
,
SLOT
(
enableDaylight
(
bool
)));
connect
(
UASManager
::
instance
(),
SIGNAL
(
homePositionChanged
(
double
,
double
,
double
)),
this
,
SLOT
(
setHome
(
double
,
double
,
double
)));
}
QGCGoogleEarthView
::~
QGCGoogleEarthView
()
...
...
@@ -353,9 +358,43 @@ void QGCGoogleEarthView::goHome()
void
QGCGoogleEarthView
::
setHome
(
double
lat
,
double
lon
,
double
alt
)
{
qDebug
()
<<
"SETTING GCS HOME IN GOOGLE MAPS"
<<
lat
<<
lon
<<
alt
;
javaScript
(
QString
(
"setGCSHome(%1,%2,%3);"
).
arg
(
lat
,
0
,
'f'
,
15
).
arg
(
lon
,
0
,
'f'
,
15
).
arg
(
alt
,
0
,
'f'
,
15
));
}
void
QGCGoogleEarthView
::
setHome
()
{
javaScript
(
QString
(
"enableSetHomeMode();"
));
}
void
QGCGoogleEarthView
::
moveToPosition
()
{
bool
ok
;
javaScript
(
"sampleCurrentPosition();"
);
double
latitude
=
documentElement
(
"currentCameraLatitude"
).
toDouble
();
double
longitude
=
documentElement
(
"currentCameraLongitude"
).
toDouble
();
QString
text
=
QInputDialog
::
getText
(
this
,
tr
(
"Please enter coordinates"
),
tr
(
"Coordinates (Lat,Lon):"
),
QLineEdit
::
Normal
,
QString
(
"%1,%2"
).
arg
(
latitude
).
arg
(
longitude
),
&
ok
);
if
(
ok
&&
!
text
.
isEmpty
())
{
QStringList
split
=
text
.
split
(
","
);
if
(
split
.
length
()
==
2
)
{
bool
convert
;
double
latitude
=
split
.
first
().
toDouble
(
&
convert
);
ok
&=
convert
;
double
longitude
=
split
.
last
().
toDouble
(
&
convert
);
ok
&=
convert
;
if
(
ok
)
{
javaScript
(
QString
(
"setLookAtLatLon(%1,%2);"
).
arg
(
latitude
,
0
,
'f'
,
15
).
arg
(
longitude
,
0
,
'f'
,
15
));
}
}
}
}
void
QGCGoogleEarthView
::
hideEvent
(
QHideEvent
*
event
)
{
Q_UNUSED
(
event
);
...
...
@@ -386,7 +425,7 @@ void QGCGoogleEarthView::showEvent(QShowEvent* event)
// Reloading the webpage, this resets Google Earth
gEarthInitialized
=
false
;
QTimer
::
singleShot
(
10
000
,
this
,
SLOT
(
initializeGoogleEarth
()));
QTimer
::
singleShot
(
3
000
,
this
,
SLOT
(
initializeGoogleEarth
()));
}
else
{
...
...
@@ -402,7 +441,7 @@ void QGCGoogleEarthView::printWinException(int no, QString str1, QString str2, Q
QVariant
QGCGoogleEarthView
::
javaScript
(
QString
javaScript
)
{
#ifdef Q_OS_MAC
if
(
!
gEarth
Initialized
)
if
(
!
jScript
Initialized
)
{
return
QVariant
(
false
);
}
...
...
@@ -439,27 +478,47 @@ QVariant QGCGoogleEarthView::documentElement(QString name)
if
(
!
jScriptInitialized
)
{
qDebug
()
<<
"TOO EARLY JAVASCRIPT CALL, ABORTING"
;
return
QVariant
(
false
);
}
else
{
QVariantList
params
;
QString
javaScript
(
"getGlobal(%1)"
);
params
.
append
(
javaScript
.
arg
(
name
));
params
.
append
(
"JScript"
);
QVariant
result
=
jScriptWin
->
dynamicCall
(
"execScript(QString, QString)"
,
params
);
qDebug
()
<<
"JScript result: "
<<
result
<<
result
.
toDouble
();
// if (documentWin)
// {
// // Get HTMLElement object
// QVariantList params;
// params.append(name);
// //QAxObject* elementWin = documentWin->dynamicCall("getElementById(QString)", params);
// QVariant result =documentWin->dynamicCall("toString()");
// qDebug() << "GOT RESULT" << result;
// return QVariant(0);//QVariant(result);
// }
if
(
documentWin
)
{
QString
resultString
;
// Get HTMLElement object
QVariantList
params
;
IHTMLDocument3
*
doc
;
documentWin
->
queryInterface
(
IID_IHTMLDocument3
,
(
void
**
)
&
doc
);
params
.
append
(
name
);
IHTMLElement
*
element
=
NULL
;
// Append alias
name
.
prepend
(
"JScript_"
);
HRESULT
res
=
doc
->
getElementById
(
QStringToBSTR
(
name
),
&
element
);
//BSTR elemString;
if
(
element
)
{
//element->get_innerHTML(&elemString);
VARIANT
var
;
var
.
vt
=
VT_BSTR
;
HRESULT
res
=
element
->
getAttribute
(
L"value"
,
0
,
&
var
);
if
(
SUCCEEDED
(
res
)
&&
(
var
.
vt
!=
VT_NULL
))
{
QByteArray
typeName
;
QVariant
qtValue
=
VARIANTToQVariant
(
var
,
typeName
);
return
qtValue
;
}
else
{
qDebug
()
<<
__FILE__
<<
__LINE__
<<
"JAVASCRIPT ATTRIBUTE"
<<
name
<<
"NOT FOUND"
;
}
}
else
{
qDebug
()
<<
__FILE__
<<
__LINE__
<<
"DID NOT GET HTML ELEMENT"
<<
name
;
}
}
}
return
QVariant
(
0
);
#endif
}
...
...
@@ -474,22 +533,18 @@ void QGCGoogleEarthView::initializeGoogleEarth()
QAxObject
*
doc
=
webViewWin
->
querySubObject
(
"Document()"
);
//IDispatch* Disp;
IDispatch
*
winDoc
=
NULL
;
IHTMLDocument2
*
document
=
NULL
;
IHTMLDocument2
*
document
=
NULL
;
//332C4425-26CB-11D0-B483-00C04FD90119 IHTMLDocument2
//25336920-03F9-11CF-8FD0-00AA00686F13 HTMLDocument
doc
->
queryInterface
(
QUuid
(
"{332C4425-26CB-11D0-B483-00C04FD90119}"
),
(
void
**
)(
&
winDoc
));
if
(
winDoc
)
{
// Security:
// CoInternetSetFeatureEnabled
// (FEATURE_LOCALMACHINE_LOCKDOWN, SET_FEATURE_ON_PROCESS, TRUE);
//
document
=
NULL
;
winDoc
->
QueryInterface
(
IID_IHTMLDocument2
,
(
void
**
)
&
document
);
IHTMLWindow2
*
window
=
NULL
;
document
->
get_parentWindow
(
&
window
);
documentWin
=
new
QAxObject
(
document
,
webViewWin
);
documentWin
=
new
QAxObject
(
document
,
webViewWin
);
jScriptWin
=
new
QAxObject
(
window
,
webViewWin
);
connect
(
jScriptWin
,
SIGNAL
(
exception
(
int
,
QString
,
QString
,
QString
)),
this
,
SLOT
(
printWinException
(
int
,
QString
,
QString
,
QString
)));
jScriptInitialized
=
true
;
...
...
@@ -499,15 +554,15 @@ void QGCGoogleEarthView::initializeGoogleEarth()
qDebug
()
<<
"COULD NOT GET DOCUMENT OBJECT! Aborting"
;
}
#endif
QTimer
::
singleShot
(
2
500
,
this
,
SLOT
(
initializeGoogleEarth
()));
QTimer
::
singleShot
(
1
500
,
this
,
SLOT
(
initializeGoogleEarth
()));
return
;
}
if
(
!
gEarthInitialized
)
{
if
(
0
==
1
)
//(!javaScript("isInitialized();
").toBool())
if
(
!
documentElement
(
"initialized
"
).
toBool
())
{
QTimer
::
singleShot
(
5
00
,
this
,
SLOT
(
initializeGoogleEarth
()));
QTimer
::
singleShot
(
3
00
,
this
,
SLOT
(
initializeGoogleEarth
()));
qDebug
()
<<
"NOT INITIALIZED, WAITING"
;
}
else
...
...
@@ -515,10 +570,7 @@ void QGCGoogleEarthView::initializeGoogleEarth()
gEarthInitialized
=
true
;
// Set home location
setHome
(
47.3769
,
8.549444
,
500
);
// Move to home location
goHome
();
setHome
(
UASManager
::
instance
()
->
getHomeLatitude
(),
UASManager
::
instance
()
->
getHomeLongitude
(),
UASManager
::
instance
()
->
getHomeAltitude
());
// Add all MAVs
QList
<
UASInterface
*>
mavs
=
UASManager
::
instance
()
->
getUASList
();
...
...
@@ -546,6 +598,15 @@ void QGCGoogleEarthView::initializeGoogleEarth()
// Go home
connect
(
ui
->
goHomeButton
,
SIGNAL
(
clicked
()),
this
,
SLOT
(
goHome
()));
// Set home
connect
(
ui
->
setHomeButton
,
SIGNAL
(
clicked
()),
this
,
SLOT
(
setHome
()));
// Visibility of set home button
connect
(
ui
->
editButton
,
SIGNAL
(
clicked
(
bool
)),
ui
->
setHomeButton
,
SLOT
(
setVisible
(
bool
)));
ui
->
setHomeButton
->
setVisible
(
ui
->
editButton
->
isChecked
());
// To Lat/Lon button
connect
(
ui
->
toLatLonButton
,
SIGNAL
(
clicked
()),
this
,
SLOT
(
moveToPosition
()));
// Cam distance slider
connect
(
ui
->
camDistanceSlider
,
SIGNAL
(
valueChanged
(
int
)),
this
,
SLOT
(
setViewRangeScaledInt
(
int
)),
Qt
::
UniqueConnection
);
...
...
@@ -569,6 +630,9 @@ void QGCGoogleEarthView::initializeGoogleEarth()
enableAtmosphere
(
ui
->
atmosphereCheckBox
->
isChecked
());
enableDaylight
(
ui
->
daylightCheckBox
->
isChecked
());
follow
(
this
->
followCamera
);
// Move to home location
goHome
();
}
}
}
...
...
@@ -668,7 +732,9 @@ void QGCGoogleEarthView::updateState()
QString
idText
=
documentElement
(
"dragWaypointIndex"
).
toString
();
if
(
idText
==
"HOME"
)
{
setHome
(
latitude
,
longitude
,
altitude
);
qDebug
()
<<
"HOME UPDATED!"
;
UASManager
::
instance
()
->
setHomePosition
(
latitude
,
longitude
,
altitude
);
ui
->
setHomeButton
->
setChecked
(
false
);
}
else
{
...
...
src/ui/map3D/QGCGoogleEarthView.h
View file @
f8d26a43
...
...
@@ -104,6 +104,10 @@ public slots:
void
goHome
();
/** @brief Set the home location */
void
setHome
(
double
lat
,
double
lon
,
double
alt
);
/** @brief Set the home location interactively in the UI */
void
setHome
();
/** @brief Move the view to a latitude / longitude position */
void
moveToPosition
();
/** @brief Allow waypoint editing */
void
enableEditMode
(
bool
mode
);
/** @brief Enable daylight/night */
...
...
src/ui/map3D/QGCGoogleEarthView.ui
View file @
f8d26a43
...
...
@@ -6,16 +6,16 @@
<rect>
<x>
0
</x>
<y>
0
</y>
<width>
1
08
9
</width>
<width>
1
40
9
</width>
<height>
302
</height>
</rect>
</property>
<property
name=
"windowTitle"
>
<string>
Form
</string>
</property>
<layout
class=
"QGridLayout"
name=
"gridLayout"
columnstretch=
"1,10,10,10,10,5,1
,100,10,10,10,10,10,5,2,2
"
>
<layout
class=
"QGridLayout"
name=
"gridLayout"
columnstretch=
"1,10,10,10,10,5,1
0,1000,10,10,10,10,10,5,2,2,0,0
"
>
<property
name=
"horizontalSpacing"
>
<number>
8
</number>
<number>
4
</number>
</property>
<property
name=
"verticalSpacing"
>
<number>
2
</number>
...
...
@@ -23,7 +23,7 @@
<property
name=
"margin"
>
<number>
2
</number>
</property>
<item
row=
"0"
column=
"0"
colspan=
"1
6
"
>
<item
row=
"0"
column=
"0"
colspan=
"1
8
"
>
<layout
class=
"QVBoxLayout"
name=
"webViewLayout"
/>
</item>
<item
row=
"1"
column=
"1"
>
...
...
@@ -43,6 +43,32 @@
</widget>
</item>
<item
row=
"1"
column=
"3"
>
<widget
class=
"QPushButton"
name=
"setHomeButton"
>
<property
name=
"text"
>
<string>
Set Home
</string>
</property>
<property
name=
"checkable"
>
<bool>
true
</bool>
</property>
</widget>
</item>
<item
row=
"1"
column=
"2"
>
<widget
class=
"QPushButton"
name=
"editButton"
>
<property
name=
"toolTip"
>
<string>
Enable waypoint and home location edit mode
</string>
</property>
<property
name=
"statusTip"
>
<string>
Enable waypoint and home location edit mode
</string>
</property>
<property
name=
"text"
>
<string>
Edit
</string>
</property>
<property
name=
"checkable"
>
<bool>
true
</bool>
</property>
</widget>
</item>
<item
row=
"1"
column=
"4"
>
<widget
class=
"QCheckBox"
name=
"trailCheckbox"
>
<property
name=
"toolTip"
>
<string>
Show MAV trajectories
</string>
...
...
@@ -58,21 +84,35 @@
</property>
</widget>
</item>
<item
row=
"1"
column=
"
13
"
>
<
spacer
name=
"horizontalSpacer
"
>
<item
row=
"1"
column=
"
5
"
>
<
widget
class=
"Line"
name=
"line
"
>
<property
name=
"orientation"
>
<enum>
Qt::
Horizont
al
</enum>
<enum>
Qt::
Vertic
al
</enum>
</property>
<property
name=
"sizeType"
>
<enum>
QSizePolicy::Expanding
</enum>
</widget>
</item>
<item
row=
"1"
column=
"6"
>
<widget
class=
"QComboBox"
name=
"camDistanceComboBox"
>
<property
name=
"toolTip"
>
<string>
Select the MAV to chase
</string>
</property>
<property
name=
"sizeHint"
stdset=
"0"
>
<size>
<width>
5
</width>
<height>
20
</height>
</size>
<property
name=
"statusTip"
>
<string>
Select the MAV to chase
</string>
</property>
</spacer>
<property
name=
"whatsThis"
>
<string>
Select the MAV to chase
</string>
</property>
<item>
<property
name=
"text"
>
<string>
MAV Distance
</string>
</property>
</item>
<item>
<property
name=
"text"
>
<string>
Ground Distance
</string>
</property>
</item>
</widget>
</item>
<item
row=
"1"
column=
"7"
>
<widget
class=
"QSlider"
name=
"camDistanceSlider"
>
...
...
@@ -115,50 +155,20 @@
</property>
</widget>
</item>
<item
row=
"1"
column=
"5"
>
<widget
class=
"QComboBox"
name=
"camDistanceComboBox"
>
<property
name=
"toolTip"
>
<string>
Select the MAV to chase
</string>
</property>
<property
name=
"statusTip"
>
<string>
Select the MAV to chase
</string>
</property>
<property
name=
"whatsThis"
>
<string>
Select the MAV to chase
</string>
<item
row=
"1"
column=
"9"
>
<widget
class=
"QPushButton"
name=
"changeViewButton"
>
<property
name=
"text"
>
<string>
Overhead
</string>
</property>
<item>
<property
name=
"text"
>
<string>
MAV Distance
</string>
</property>
</item>
<item>
<property
name=
"text"
>
<string>
Ground Distance
</string>
</property>
</item>
</widget>
</item>
<item
row=
"1"
column=
"
4
"
>
<widget
class=
"Line"
name=
"line"
>
<item
row=
"1"
column=
"
10
"
>
<widget
class=
"Line"
name=
"line
_2
"
>
<property
name=
"orientation"
>
<enum>
Qt::Vertical
</enum>
</property>
</widget>
</item>
<item
row=
"1"
column=
"12"
>
<widget
class=
"QPushButton"
name=
"resetButton"
>
<property
name=
"text"
>
<string>
Reset
</string>
</property>
</widget>
</item>
<item
row=
"1"
column=
"9"
>
<widget
class=
"QPushButton"
name=
"changeViewButton"
>
<property
name=
"text"
>
<string>
Overhead
</string>
</property>
</widget>
</item>
<item
row=
"1"
column=
"11"
>
<widget
class=
"QPushButton"
name=
"clearTrailsButton"
>
<property
name=
"toolTip"
>
...
...
@@ -175,28 +185,28 @@
</property>
</widget>
</item>
<item
row=
"1"
column=
"1
0
"
>
<widget
class=
"
Line"
name=
"line_2
"
>
<property
name=
"
orientation
"
>
<
enum>
Qt::Vertical
</enum
>
<item
row=
"1"
column=
"1
2
"
>
<widget
class=
"
QPushButton"
name=
"resetButton
"
>
<property
name=
"
text
"
>
<
string>
Reset
</string
>
</property>
</widget>
</item>
<item
row=
"1"
column=
"2"
>
<widget
class=
"QPushButton"
name=
"editButton"
>
<property
name=
"toolTip"
>
<string>
Enable waypoint and home location edit mode
</string>
</property>
<property
name=
"statusTip"
>
<string>
Enable waypoint and home location edit mode
</string>
<item
row=
"1"
column=
"13"
>
<spacer
name=
"horizontalSpacer"
>
<property
name=
"orientation"
>
<enum>
Qt::Horizontal
</enum>
</property>
<property
name=
"
text
"
>
<
string>
Edit
</string
>
<property
name=
"
sizeType
"
>
<
enum>
QSizePolicy::MinimumExpanding
</enum
>
</property>
<property
name=
"checkable"
>
<bool>
true
</bool>
<property
name=
"sizeHint"
stdset=
"0"
>
<size>
<width>
10
</width>
<height>
20
</height>
</size>
</property>
</
widget
>
</
spacer
>
</item>
<item
row=
"1"
column=
"14"
>
<widget
class=
"QCheckBox"
name=
"atmosphereCheckBox"
>
...
...
@@ -224,6 +234,13 @@
</property>
</widget>
</item>
<item
row=
"1"
column=
"0"
>
<widget
class=
"QPushButton"
name=
"toLatLonButton"
>
<property
name=
"text"
>
<string>
Lat/Lon
</string>
</property>
</widget>
</item>
</layout>
</widget>
<resources/>
...
...
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