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
537463c5
Commit
537463c5
authored
Aug 10, 2017
by
Don Gagne
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
New Mavlink 2 spec GeoFence and Rally Point support
parent
a11b1dbf
Changes
53
Hide whitespace changes
Inline
Side-by-side
Showing
53 changed files
with
2806 additions
and
2228 deletions
+2806
-2228
qgroundcontrol.pro
qgroundcontrol.pro
+8
-6
qgroundcontrol.qrc
qgroundcontrol.qrc
+2
-0
APMFirmwarePlugin.h
src/FirmwarePlugin/APM/APMFirmwarePlugin.h
+0
-4
APMGeoFenceManager.cc
src/FirmwarePlugin/APM/APMGeoFenceManager.cc
+0
-337
APMGeoFenceManager.h
src/FirmwarePlugin/APM/APMGeoFenceManager.h
+0
-78
APMRallyPointManager.cc
src/FirmwarePlugin/APM/APMRallyPointManager.cc
+0
-166
APMRallyPointManager.h
src/FirmwarePlugin/APM/APMRallyPointManager.h
+0
-51
ArduCopterFirmwarePlugin.cc
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
+0
-6
ArduCopterFirmwarePlugin.h
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
+0
-1
FirmwarePlugin.h
src/FirmwarePlugin/FirmwarePlugin.h
+0
-9
PX4FirmwarePlugin.h
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
+0
-2
PX4GeoFenceManager.cc
src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc
+0
-60
PX4GeoFenceManager.h
src/FirmwarePlugin/PX4/PX4GeoFenceManager.h
+0
-41
qmldir
src/FlightMap/qmldir
+0
-3
GeoFenceController.cc
src/MissionManager/GeoFenceController.cc
+134
-65
GeoFenceController.h
src/MissionManager/GeoFenceController.h
+33
-33
GeoFenceManager.cc
src/MissionManager/GeoFenceManager.cc
+155
-16
GeoFenceManager.h
src/MissionManager/GeoFenceManager.h
+33
-42
MissionController.h
src/MissionManager/MissionController.h
+1
-0
MissionManager.cc
src/MissionManager/MissionManager.cc
+2
-917
MissionManager.h
src/MissionManager/MissionManager.h
+2
-127
PlanElementController.h
src/MissionManager/PlanElementController.h
+7
-4
PlanManager.cc
src/MissionManager/PlanManager.cc
+970
-0
PlanManager.h
src/MissionManager/PlanManager.h
+154
-0
PlanMasterController.cc
src/MissionManager/PlanMasterController.cc
+37
-11
QGCFenceCircle.cc
src/MissionManager/QGCFenceCircle.cc
+88
-0
QGCFenceCircle.h
src/MissionManager/QGCFenceCircle.h
+55
-0
QGCFencePolygon.cc
src/MissionManager/QGCFencePolygon.cc
+81
-0
QGCFencePolygon.h
src/MissionManager/QGCFencePolygon.h
+55
-0
QGCMapCircle.Facts.json
src/MissionManager/QGCMapCircle.Facts.json
+10
-0
QGCMapCircle.cc
src/MissionManager/QGCMapCircle.cc
+144
-0
QGCMapCircle.h
src/MissionManager/QGCMapCircle.h
+81
-0
QGCMapCircleVisuals.qml
src/MissionManager/QGCMapCircleVisuals.qml
+140
-0
QGCMapPolygon.cc
src/MissionManager/QGCMapPolygon.cc
+49
-10
QGCMapPolygon.h
src/MissionManager/QGCMapPolygon.h
+16
-8
QGCMapPolygonTest.cc
src/MissionManager/QGCMapPolygonTest.cc
+1
-1
QGCMapPolygonVisuals.qml
src/MissionManager/QGCMapPolygonVisuals.qml
+6
-6
RallyPointController.cc
src/MissionManager/RallyPointController.cc
+6
-3
RallyPointController.h
src/MissionManager/RallyPointController.h
+1
-3
RallyPointManager.cc
src/MissionManager/RallyPointManager.cc
+5
-0
RallyPointManager.h
src/MissionManager/RallyPointManager.h
+3
-2
GeoFenceEditor.qml
src/PlanView/GeoFenceEditor.qml
+290
-71
GeoFenceMapVisuals.qml
src/PlanView/GeoFenceMapVisuals.qml
+55
-35
PlanView.qml
src/PlanView/PlanView.qml
+1
-1
RallyPointEditorHeader.qml
src/PlanView/RallyPointEditorHeader.qml
+1
-1
RallyPointMapVisuals.qml
src/PlanView/RallyPointMapVisuals.qml
+1
-1
QGroundControl.Controls.qmldir
src/QmlControls/QGroundControl.Controls.qmldir
+1
-0
Vehicle.cc
src/Vehicle/Vehicle.cc
+37
-13
Vehicle.h
src/Vehicle/Vehicle.h
+6
-10
MockLink.cc
src/comm/MockLink.cc
+1
-1
MockLinkMissionItemHandler.cc
src/comm/MockLinkMissionItemHandler.cc
+121
-80
MockLinkMissionItemHandler.h
src/comm/MockLinkMissionItemHandler.h
+7
-3
UnitTest.cc
src/qgcunittest/UnitTest.cc
+6
-0
No files found.
qgroundcontrol.pro
View file @
537463c5
...
...
@@ -509,7 +509,11 @@ HEADERS += \
src
/
MissionManager
/
MissionManager
.
h
\
src
/
MissionManager
/
MissionSettingsItem
.
h
\
src
/
MissionManager
/
PlanElementController
.
h
\
src
/
MissionManager
/
PlanManager
.
h
\
src
/
MissionManager
/
PlanMasterController
.
h
\
src
/
MissionManager
/
QGCFenceCircle
.
h
\
src
/
MissionManager
/
QGCFencePolygon
.
h
\
src
/
MissionManager
/
QGCMapCircle
.
h
\
src
/
MissionManager
/
QGCMapPolygon
.
h
\
src
/
MissionManager
/
RallyPoint
.
h
\
src
/
MissionManager
/
RallyPointController
.
h
\
...
...
@@ -689,7 +693,11 @@ SOURCES += \
src
/
MissionManager
/
MissionManager
.
cc
\
src
/
MissionManager
/
MissionSettingsItem
.
cc
\
src
/
MissionManager
/
PlanElementController
.
cc
\
src
/
MissionManager
/
PlanManager
.
cc
\
src
/
MissionManager
/
PlanMasterController
.
cc
\
src
/
MissionManager
/
QGCFenceCircle
.
cc
\
src
/
MissionManager
/
QGCFencePolygon
.
cc
\
src
/
MissionManager
/
QGCMapCircle
.
cc
\
src
/
MissionManager
/
QGCMapPolygon
.
cc
\
src
/
MissionManager
/
RallyPoint
.
cc
\
src
/
MissionManager
/
RallyPointController
.
cc
\
...
...
@@ -906,9 +914,7 @@ APMFirmwarePlugin {
src
/
AutoPilotPlugins
/
APM
/
APMSensorsComponentController
.
h
\
src
/
AutoPilotPlugins
/
APM
/
APMTuningComponent
.
h
\
src
/
FirmwarePlugin
/
APM
/
APMFirmwarePlugin
.
h
\
src
/
FirmwarePlugin
/
APM
/
APMGeoFenceManager
.
h
\
src
/
FirmwarePlugin
/
APM
/
APMParameterMetaData
.
h
\
src
/
FirmwarePlugin
/
APM
/
APMRallyPointManager
.
h
\
src
/
FirmwarePlugin
/
APM
/
ArduCopterFirmwarePlugin
.
h
\
src
/
FirmwarePlugin
/
APM
/
ArduPlaneFirmwarePlugin
.
h
\
src
/
FirmwarePlugin
/
APM
/
ArduRoverFirmwarePlugin
.
h
\
...
...
@@ -933,9 +939,7 @@ APMFirmwarePlugin {
src
/
AutoPilotPlugins
/
APM
/
APMSensorsComponentController
.
cc
\
src
/
AutoPilotPlugins
/
APM
/
APMTuningComponent
.
cc
\
src
/
FirmwarePlugin
/
APM
/
APMFirmwarePlugin
.
cc
\
src
/
FirmwarePlugin
/
APM
/
APMGeoFenceManager
.
cc
\
src
/
FirmwarePlugin
/
APM
/
APMParameterMetaData
.
cc
\
src
/
FirmwarePlugin
/
APM
/
APMRallyPointManager
.
cc
\
src
/
FirmwarePlugin
/
APM
/
ArduCopterFirmwarePlugin
.
cc
\
src
/
FirmwarePlugin
/
APM
/
ArduPlaneFirmwarePlugin
.
cc
\
src
/
FirmwarePlugin
/
APM
/
ArduRoverFirmwarePlugin
.
cc
\
...
...
@@ -974,7 +978,6 @@ PX4FirmwarePlugin {
src
/
AutoPilotPlugins
/
PX4
/
SensorsComponent
.
h
\
src
/
AutoPilotPlugins
/
PX4
/
SensorsComponentController
.
h
\
src
/
FirmwarePlugin
/
PX4
/
PX4FirmwarePlugin
.
h
\
src
/
FirmwarePlugin
/
PX4
/
PX4GeoFenceManager
.
h
\
src
/
FirmwarePlugin
/
PX4
/
PX4ParameterMetaData
.
h
\
SOURCES
+=
\
...
...
@@ -995,7 +998,6 @@ PX4FirmwarePlugin {
src
/
AutoPilotPlugins
/
PX4
/
SensorsComponent
.
cc
\
src
/
AutoPilotPlugins
/
PX4
/
SensorsComponentController
.
cc
\
src
/
FirmwarePlugin
/
PX4
/
PX4FirmwarePlugin
.
cc
\
src
/
FirmwarePlugin
/
PX4
/
PX4GeoFenceManager
.
cc
\
src
/
FirmwarePlugin
/
PX4
/
PX4ParameterMetaData
.
cc
\
}
...
...
qgroundcontrol.qrc
View file @
537463c5
...
...
@@ -90,6 +90,7 @@
<file alias="QGroundControl/Controls/QGCLabel.qml">src/QmlControls/QGCLabel.qml</file>
<file alias="QGroundControl/Controls/QGCListView.qml">src/QmlControls/QGCListView.qml</file>
<file alias="QGroundControl/Controls/QGCMapLabel.qml">src/QmlControls/QGCMapLabel.qml</file>
<file alias="QGroundControl/Controls/QGCMapCircleVisuals.qml">src/MissionManager/QGCMapCircleVisuals.qml</file>
<file alias="QGroundControl/Controls/QGCMapPolygonVisuals.qml">src/MissionManager/QGCMapPolygonVisuals.qml</file>
<file alias="QGroundControl/Controls/QGCMouseArea.qml">src/QmlControls/QGCMouseArea.qml</file>
<file alias="QGroundControl/Controls/QGCMovableItem.qml">src/QmlControls/QGCMovableItem.qml</file>
...
...
@@ -199,6 +200,7 @@
<file alias="AutoConnect.SettingsGroup.json">src/Settings/AutoConnect.SettingsGroup.json</file>
<file alias="FlightMap.SettingsGroup.json">src/Settings/FlightMap.SettingsGroup.json</file>
<file alias="Guided.SettingsGroup.json">src/Settings/Guided.SettingsGroup.json</file>
<file alias="QGCMapCircle.Facts.json">src/MissionManager/QGCMapCircle.Facts.json</file>
<file alias="RTK.SettingsGroup.json">src/Settings/RTK.SettingsGroup.json</file>
<file alias="Survey.SettingsGroup.json">src/MissionManager/Survey.SettingsGroup.json</file>
<file alias="Units.SettingsGroup.json">src/Settings/Units.SettingsGroup.json</file>
...
...
src/FirmwarePlugin/APM/APMFirmwarePlugin.h
View file @
537463c5
...
...
@@ -17,8 +17,6 @@
#include "FirmwarePlugin.h"
#include "QGCLoggingCategory.h"
#include "APMParameterMetaData.h"
#include "APMGeoFenceManager.h"
#include "APMRallyPointManager.h"
#include <QAbstractSocket>
...
...
@@ -93,8 +91,6 @@ public:
QString
internalParameterMetaDataFile
(
Vehicle
*
vehicle
)
final
;
void
getParameterMetaDataVersionInfo
(
const
QString
&
metaDataFile
,
int
&
majorVersion
,
int
&
minorVersion
)
final
{
APMParameterMetaData
::
getParameterMetaDataVersionInfo
(
metaDataFile
,
majorVersion
,
minorVersion
);
}
QObject
*
loadParameterMetaData
(
const
QString
&
metaDataFile
)
final
;
GeoFenceManager
*
newGeoFenceManager
(
Vehicle
*
vehicle
)
final
{
return
new
APMGeoFenceManager
(
vehicle
);
}
RallyPointManager
*
newRallyPointManager
(
Vehicle
*
vehicle
)
final
{
return
new
APMRallyPointManager
(
vehicle
);
}
QString
brandImageIndoor
(
const
Vehicle
*
vehicle
)
const
override
{
Q_UNUSED
(
vehicle
);
return
QStringLiteral
(
"/qmlimages/APM/BrandImage"
);
}
QString
brandImageOutdoor
(
const
Vehicle
*
vehicle
)
const
override
{
Q_UNUSED
(
vehicle
);
return
QStringLiteral
(
"/qmlimages/APM/BrandImage"
);
}
...
...
src/FirmwarePlugin/APM/APMGeoFenceManager.cc
deleted
100644 → 0
View file @
a11b1dbf
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "APMGeoFenceManager.h"
#include "Vehicle.h"
#include "FirmwarePlugin.h"
#include "MAVLinkProtocol.h"
#include "QGCApplication.h"
#include "ParameterManager.h"
#include "QmlObjectListModel.h"
#include "QGCQGeoCoordinate.h"
const
char
*
APMGeoFenceManager
::
_fenceTotalParam
=
"FENCE_TOTAL"
;
const
char
*
APMGeoFenceManager
::
_fenceActionParam
=
"FENCE_ACTION"
;
const
char
*
APMGeoFenceManager
::
_fenceEnableParam
=
"FENCE_ENABLE"
;
APMGeoFenceManager
::
APMGeoFenceManager
(
Vehicle
*
vehicle
)
:
GeoFenceManager
(
vehicle
)
,
_fenceSupported
(
false
)
,
_circleEnabled
(
false
)
,
_polygonSupported
(
false
)
,
_polygonEnabled
(
false
)
,
_breachReturnSupported
(
vehicle
->
fixedWing
())
,
_firstParamLoadComplete
(
false
)
,
_readTransactionInProgress
(
false
)
,
_writeTransactionInProgress
(
false
)
,
_fenceTypeFact
(
NULL
)
,
_fenceEnableFact
(
NULL
)
,
_circleRadiusFact
(
NULL
)
{
connect
(
_vehicle
,
&
Vehicle
::
mavlinkMessageReceived
,
this
,
&
APMGeoFenceManager
::
_mavlinkMessageReceived
);
connect
(
_vehicle
->
parameterManager
(),
&
ParameterManager
::
parametersReadyChanged
,
this
,
&
APMGeoFenceManager
::
_parametersReady
);
if
(
_vehicle
->
parameterManager
()
->
parametersReady
())
{
_parametersReady
();
}
}
APMGeoFenceManager
::~
APMGeoFenceManager
()
{
}
void
APMGeoFenceManager
::
sendToVehicle
(
const
QGeoCoordinate
&
breachReturn
,
QmlObjectListModel
&
polygon
)
{
if
(
_vehicle
->
isOfflineEditingVehicle
())
{
return
;
}
if
(
_readTransactionInProgress
)
{
_sendError
(
InternalError
,
QStringLiteral
(
"Geo-Fence write attempted while read in progress."
));
return
;
}
if
(
!
_fenceSupported
)
{
return
;
}
// Validate
int
validatedPolygonCount
=
0
;
if
(
polygon
.
count
()
>=
3
)
{
validatedPolygonCount
=
polygon
.
count
();
}
if
(
polygon
.
count
()
>
std
::
numeric_limits
<
uint8_t
>::
max
())
{
_sendError
(
TooManyPoints
,
QStringLiteral
(
"Geo-Fence polygon has too many points: %1."
).
arg
(
_polygon
.
count
()));
validatedPolygonCount
=
0
;
}
_breachReturnPoint
=
breachReturn
;
_polygon
.
clear
();
if
(
validatedPolygonCount
)
{
for
(
int
i
=
0
;
i
<
polygon
.
count
();
i
++
)
{
_polygon
.
append
(
polygon
.
value
<
QGCQGeoCoordinate
*>
(
i
)
->
coordinate
());
}
}
// Total point count, +1 polygon close in last index, +1 for breach in index 0
_cWriteFencePoints
=
validatedPolygonCount
?
validatedPolygonCount
+
1
+
1
:
0
;
qCDebug
(
GeoFenceManagerLog
)
<<
"APMGeoFenceManager::sendToVehicle validatedPolygonCount:_cWriteFencePoints"
<<
validatedPolygonCount
<<
_cWriteFencePoints
;
_vehicle
->
parameterManager
()
->
getParameter
(
FactSystem
::
defaultComponentId
,
_fenceTotalParam
)
->
setRawValue
(
_cWriteFencePoints
);
// FIXME: No validation of correct fence received
for
(
uint8_t
index
=
0
;
index
<
_cWriteFencePoints
;
index
++
)
{
_sendFencePoint
(
index
);
}
emit
sendComplete
(
false
/* error */
);
}
void
APMGeoFenceManager
::
loadFromVehicle
(
void
)
{
if
(
_vehicle
->
isOfflineEditingVehicle
()
||
_readTransactionInProgress
)
{
return
;
}
_breachReturnPoint
=
QGeoCoordinate
();
_polygon
.
clear
();
if
(
!
_fenceSupported
)
{
emit
loadComplete
(
_breachReturnPoint
,
_polygon
);
return
;
}
// Point 0: Breach return point (always sent, but supported by ArduPlane only)
// Point [1,N]: Polygon points
// Point N+1: Close polygon point (same as point 1)
int
cFencePoints
=
_vehicle
->
parameterManager
()
->
getParameter
(
FactSystem
::
defaultComponentId
,
_fenceTotalParam
)
->
rawValue
().
toInt
();
int
minFencePoints
=
5
;
qCDebug
(
GeoFenceManagerLog
)
<<
"APMGeoFenceManager::loadFromVehicle"
<<
cFencePoints
;
if
(
cFencePoints
==
0
)
{
// No fence
emit
loadComplete
(
_breachReturnPoint
,
_polygon
);
return
;
}
if
(
cFencePoints
<
0
||
(
cFencePoints
>
0
&&
cFencePoints
<
minFencePoints
))
{
_sendError
(
TooFewPoints
,
QStringLiteral
(
"Geo-Fence information from Vehicle has too few points: %1"
).
arg
(
cFencePoints
));
return
;
}
if
(
cFencePoints
>
std
::
numeric_limits
<
uint8_t
>::
max
())
{
_sendError
(
TooManyPoints
,
QStringLiteral
(
"Geo-Fence information from Vehicle has too many points: %1"
).
arg
(
cFencePoints
));
return
;
}
_readTransactionInProgress
=
true
;
_cReadFencePoints
=
cFencePoints
;
_currentFencePoint
=
0
;
_requestFencePoint
(
_currentFencePoint
);
}
/// Called when a new mavlink message for our vehicle is received
void
APMGeoFenceManager
::
_mavlinkMessageReceived
(
const
mavlink_message_t
&
message
)
{
if
(
message
.
msgid
==
MAVLINK_MSG_ID_FENCE_POINT
)
{
mavlink_fence_point_t
fencePoint
;
mavlink_msg_fence_point_decode
(
&
message
,
&
fencePoint
);
qCDebug
(
GeoFenceManagerLog
)
<<
"From vehicle fence_point: idx:lat:lng"
<<
fencePoint
.
idx
<<
fencePoint
.
lat
<<
fencePoint
.
lng
;
if
(
fencePoint
.
idx
!=
_currentFencePoint
)
{
// FIXME: Protocol out of whack
_readTransactionInProgress
=
false
;
emit
inProgressChanged
(
inProgress
());
qCWarning
(
GeoFenceManagerLog
)
<<
"Indices out of sync"
<<
fencePoint
.
idx
<<
_currentFencePoint
;
return
;
}
if
(
fencePoint
.
idx
==
0
)
{
_breachReturnPoint
=
QGeoCoordinate
(
fencePoint
.
lat
,
fencePoint
.
lng
);
qCDebug
(
GeoFenceManagerLog
)
<<
"From vehicle: breach return point"
<<
_breachReturnPoint
;
_requestFencePoint
(
++
_currentFencePoint
);
}
else
if
(
fencePoint
.
idx
<
_cReadFencePoints
-
1
)
{
QGeoCoordinate
polyCoord
(
fencePoint
.
lat
,
fencePoint
.
lng
);
_polygon
.
append
(
polyCoord
);
qCDebug
(
GeoFenceManagerLog
)
<<
"From vehicle: polygon point"
<<
fencePoint
.
idx
<<
polyCoord
;
if
(
fencePoint
.
idx
<
_cReadFencePoints
-
2
)
{
// Still more points to request
_requestFencePoint
(
++
_currentFencePoint
);
}
else
{
// We've finished collecting fence points
qCDebug
(
GeoFenceManagerLog
)
<<
"Fence point load complete"
;
_readTransactionInProgress
=
false
;
emit
loadComplete
(
_breachReturnPoint
,
_polygon
);
emit
inProgressChanged
(
inProgress
());
}
}
}
}
void
APMGeoFenceManager
::
_requestFencePoint
(
uint8_t
pointIndex
)
{
mavlink_message_t
msg
;
MAVLinkProtocol
*
mavlink
=
qgcApp
()
->
toolbox
()
->
mavlinkProtocol
();
qCDebug
(
GeoFenceManagerLog
)
<<
"APMGeoFenceManager::_requestFencePoint"
<<
pointIndex
;
mavlink_msg_fence_fetch_point_pack_chan
(
mavlink
->
getSystemId
(),
mavlink
->
getComponentId
(),
_vehicle
->
priorityLink
()
->
mavlinkChannel
(),
&
msg
,
_vehicle
->
id
(),
_vehicle
->
defaultComponentId
(),
pointIndex
);
_vehicle
->
sendMessageOnLink
(
_vehicle
->
priorityLink
(),
msg
);
}
void
APMGeoFenceManager
::
_sendFencePoint
(
uint8_t
pointIndex
)
{
mavlink_message_t
msg
;
MAVLinkProtocol
*
mavlink
=
qgcApp
()
->
toolbox
()
->
mavlinkProtocol
();
QGeoCoordinate
fenceCoord
;
if
(
pointIndex
==
0
)
{
fenceCoord
=
breachReturnPoint
();
}
else
if
(
pointIndex
==
_cWriteFencePoints
-
1
)
{
// Polygon close point
fenceCoord
=
_polygon
[
0
];
}
else
{
// Polygon point
fenceCoord
=
_polygon
[
pointIndex
-
1
];
}
// Total point count, +1 polygon close in last index, +1 for breach in index 0
uint8_t
totalPointCount
=
_polygon
.
count
()
+
1
+
1
;
mavlink_msg_fence_point_pack_chan
(
mavlink
->
getSystemId
(),
mavlink
->
getComponentId
(),
_vehicle
->
priorityLink
()
->
mavlinkChannel
(),
&
msg
,
_vehicle
->
id
(),
_vehicle
->
defaultComponentId
(),
pointIndex
,
// Index of point to set
totalPointCount
,
fenceCoord
.
latitude
(),
fenceCoord
.
longitude
());
_vehicle
->
sendMessageOnLink
(
_vehicle
->
priorityLink
(),
msg
);
}
bool
APMGeoFenceManager
::
inProgress
(
void
)
const
{
return
_readTransactionInProgress
||
_writeTransactionInProgress
;
}
void
APMGeoFenceManager
::
_setCircleEnabled
(
bool
circleEnabled
)
{
if
(
circleEnabled
!=
_circleEnabled
)
{
_circleEnabled
=
circleEnabled
;
emit
circleEnabledChanged
(
circleEnabled
);
}
}
void
APMGeoFenceManager
::
_setPolygonEnabled
(
bool
polygonEnabled
)
{
if
(
polygonEnabled
!=
_polygonEnabled
)
{
_polygonEnabled
=
polygonEnabled
;
emit
polygonEnabledChanged
(
polygonEnabled
);
}
}
void
APMGeoFenceManager
::
_updateEnabledFlags
(
void
)
{
bool
fenceEnabled
=
false
;
if
(
_fenceSupported
)
{
if
(
_fenceEnableFact
)
{
fenceEnabled
=
_fenceEnableFact
->
rawValue
().
toBool
();
}
else
{
fenceEnabled
=
true
;
}
bool
newCircleEnabled
=
fenceEnabled
&&
_fenceTypeFact
&&
(
_fenceTypeFact
->
rawValue
().
toInt
()
&
2
);
_setCircleEnabled
(
newCircleEnabled
);
bool
newPolygonEnabled
=
_fenceSupported
&&
fenceEnabled
&&
((
_vehicle
->
multiRotor
()
&&
_fenceTypeFact
&&
(
_fenceTypeFact
->
rawValue
().
toInt
()
&
4
))
||
_vehicle
->
fixedWing
());
_setPolygonEnabled
(
newPolygonEnabled
);
}
else
{
_setCircleEnabled
(
false
);
_setPolygonEnabled
(
false
);
}
}
void
APMGeoFenceManager
::
_parametersReady
(
void
)
{
if
(
!
_firstParamLoadComplete
)
{
_firstParamLoadComplete
=
true
;
_fenceSupported
=
_vehicle
->
parameterManager
()
->
parameterExists
(
FactSystem
::
defaultComponentId
,
_fenceTotalParam
)
&&
_vehicle
->
parameterManager
()
->
parameterExists
(
FactSystem
::
defaultComponentId
,
_fenceActionParam
)
&&
!
qgcApp
()
->
runningUnitTests
();
if
(
_fenceSupported
)
{
QStringList
paramNames
;
QStringList
paramLabels
;
_polygonSupported
=
true
;
if
(
_vehicle
->
parameterManager
()
->
parameterExists
(
FactSystem
::
defaultComponentId
,
_fenceEnableParam
))
{
_fenceEnableFact
=
_vehicle
->
parameterManager
()
->
getParameter
(
FactSystem
::
defaultComponentId
,
_fenceEnableParam
);
connect
(
_fenceEnableFact
,
&
Fact
::
rawValueChanged
,
this
,
&
APMGeoFenceManager
::
_updateEnabledFlags
);
}
if
(
_vehicle
->
multiRotor
())
{
_breachReturnSupported
=
false
;
_fenceTypeFact
=
_vehicle
->
parameterManager
()
->
getParameter
(
FactSystem
::
defaultComponentId
,
QStringLiteral
(
"FENCE_TYPE"
));
connect
(
_fenceTypeFact
,
&
Fact
::
rawValueChanged
,
this
,
&
APMGeoFenceManager
::
_updateEnabledFlags
);
_circleRadiusFact
=
_vehicle
->
parameterManager
()
->
getParameter
(
FactSystem
::
defaultComponentId
,
QStringLiteral
(
"FENCE_RADIUS"
));
paramNames
<<
QStringLiteral
(
"FENCE_ENABLE"
)
<<
QStringLiteral
(
"FENCE_TYPE"
)
<<
QStringLiteral
(
"FENCE_ACTION"
)
<<
QStringLiteral
(
"FENCE_ALT_MAX"
)
<<
QStringLiteral
(
"FENCE_RADIUS"
)
<<
QStringLiteral
(
"FENCE_MARGIN"
);
paramLabels
<<
QStringLiteral
(
"Enabled:"
)
<<
QStringLiteral
(
"Type:"
)
<<
QStringLiteral
(
"Breach Action:"
)
<<
QStringLiteral
(
"Max Altitude:"
)
<<
QStringLiteral
(
"Radius:"
)
<<
QStringLiteral
(
"Margin:"
);
}
if
(
_vehicle
->
fixedWing
())
{
_breachReturnSupported
=
true
;
paramNames
<<
QStringLiteral
(
"FENCE_ACTION"
)
<<
QStringLiteral
(
"FENCE_MINALT"
)
<<
QStringLiteral
(
"FENCE_MAXALT"
)
<<
QStringLiteral
(
"FENCE_RETALT"
)
<<
QStringLiteral
(
"FENCE_AUTOENABLE"
)
<<
QStringLiteral
(
"FENCE_RET_RALLY"
);
paramLabels
<<
QStringLiteral
(
"Breach Action:"
)
<<
QStringLiteral
(
"Min Altitude:"
)
<<
QStringLiteral
(
"Max Altitude:"
)
<<
QStringLiteral
(
"Return Altitude:"
)
<<
QStringLiteral
(
"Auto-Enable:"
)
<<
QStringLiteral
(
"Return to Rally:"
);
}
_params
.
clear
();
_paramLabels
.
clear
();
for
(
int
i
=
0
;
i
<
paramNames
.
count
();
i
++
)
{
QString
paramName
=
paramNames
[
i
];
if
(
_vehicle
->
parameterManager
()
->
parameterExists
(
FactSystem
::
defaultComponentId
,
paramName
))
{
Fact
*
paramFact
=
_vehicle
->
parameterManager
()
->
getParameter
(
FactSystem
::
defaultComponentId
,
paramName
);
_params
<<
QVariant
::
fromValue
(
paramFact
);
_paramLabels
<<
paramLabels
[
i
];
}
}
emit
paramsChanged
(
_params
);
emit
paramLabelsChanged
(
_paramLabels
);
emit
breachReturnSupportedChanged
(
_breachReturnSupported
);
emit
polygonSupportedChanged
(
_polygonSupported
);
}
_updateEnabledFlags
();
}
}
void
APMGeoFenceManager
::
removeAll
(
void
)
{
qCDebug
(
GeoFenceManagerLog
)
<<
"APMGeoFenceManager::removeAll"
;
QmlObjectListModel
emptyPolygon
;
sendToVehicle
(
_breachReturnPoint
,
emptyPolygon
);
emit
removeAllComplete
(
false
/* error */
);
}
src/FirmwarePlugin/APM/APMGeoFenceManager.h
deleted
100644 → 0
View file @
a11b1dbf
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#ifndef APMGeoFenceManager_H
#define APMGeoFenceManager_H
#include "GeoFenceManager.h"
#include "QGCMAVLink.h"
#include "FactSystem.h"
class
QmlObjectListModel
;
class
APMGeoFenceManager
:
public
GeoFenceManager
{
Q_OBJECT
public:
APMGeoFenceManager
(
Vehicle
*
vehicle
);
~
APMGeoFenceManager
();
// Overrides from GeoFenceManager
bool
inProgress
(
void
)
const
final
;
void
loadFromVehicle
(
void
)
final
;
void
sendToVehicle
(
const
QGeoCoordinate
&
breachReturn
,
QmlObjectListModel
&
polygon
)
final
;
bool
polygonSupported
(
void
)
const
final
{
return
_polygonSupported
;
}
bool
polygonEnabled
(
void
)
const
final
{
return
_polygonEnabled
;
}
bool
breachReturnSupported
(
void
)
const
final
{
return
_breachReturnSupported
;
}
bool
circleEnabled
(
void
)
const
{
return
_circleEnabled
;
}
Fact
*
circleRadiusFact
(
void
)
const
{
return
_circleRadiusFact
;
}
QVariantList
params
(
void
)
const
final
{
return
_params
;
}
QStringList
paramLabels
(
void
)
const
final
{
return
_paramLabels
;
}
void
removeAll
(
void
)
final
;
private
slots
:
void
_mavlinkMessageReceived
(
const
mavlink_message_t
&
message
);
void
_parametersReady
(
void
);
private:
void
_requestFencePoint
(
uint8_t
pointIndex
);
void
_sendFencePoint
(
uint8_t
pointIndex
);
void
_updateEnabledFlags
(
void
);
void
_setCircleEnabled
(
bool
circleEnabled
);
void
_setPolygonEnabled
(
bool
polygonEnabled
);
private:
bool
_fenceSupported
;
bool
_circleEnabled
;
bool
_polygonSupported
;
bool
_polygonEnabled
;
bool
_breachReturnSupported
;
bool
_firstParamLoadComplete
;
QVariantList
_params
;
QStringList
_paramLabels
;
bool
_readTransactionInProgress
;
bool
_writeTransactionInProgress
;
uint8_t
_cReadFencePoints
;
uint8_t
_cWriteFencePoints
;
uint8_t
_currentFencePoint
;
Fact
*
_fenceTypeFact
;
Fact
*
_fenceEnableFact
;
Fact
*
_circleRadiusFact
;
static
const
char
*
_fenceTotalParam
;
static
const
char
*
_fenceActionParam
;
static
const
char
*
_fenceEnableParam
;
};
#endif
src/FirmwarePlugin/APM/APMRallyPointManager.cc
deleted
100644 → 0
View file @
a11b1dbf
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "APMRallyPointManager.h"
#include "Vehicle.h"
#include "FirmwarePlugin.h"
#include "MAVLinkProtocol.h"
#include "QGCApplication.h"
#include "ParameterManager.h"
const
char
*
APMRallyPointManager
::
_rallyTotalParam
=
"RALLY_TOTAL"
;
APMRallyPointManager
::
APMRallyPointManager
(
Vehicle
*
vehicle
)
:
RallyPointManager
(
vehicle
)
,
_readTransactionInProgress
(
false
)
,
_writeTransactionInProgress
(
false
)
{
connect
(
_vehicle
,
&
Vehicle
::
mavlinkMessageReceived
,
this
,
&
APMRallyPointManager
::
_mavlinkMessageReceived
);
}
APMRallyPointManager
::~
APMRallyPointManager
()
{
}
void
APMRallyPointManager
::
sendToVehicle
(
const
QList
<
QGeoCoordinate
>&
rgPoints
)
{
if
(
_vehicle
->
isOfflineEditingVehicle
()
||
!
rallyPointsSupported
())
{
return
;
}
if
(
_readTransactionInProgress
)
{
_sendError
(
InternalError
,
QStringLiteral
(
"Rally Point write attempted while read in progress."
));
return
;
}
_vehicle
->
parameterManager
()
->
getParameter
(
FactSystem
::
defaultComponentId
,
_rallyTotalParam
)
->
setRawValue
(
rgPoints
.
count
());
// FIXME: No validation of correct point received
_rgPoints
=
rgPoints
;
for
(
uint8_t
index
=
0
;
index
<
_rgPoints
.
count
();
index
++
)
{
_sendRallyPoint
(
index
);
}
emit
sendComplete
(
false
/* error */
);
}
void
APMRallyPointManager
::
loadFromVehicle
(
void
)
{
if
(
_vehicle
->
isOfflineEditingVehicle
()
||
_readTransactionInProgress
)
{
return
;
}
_rgPoints
.
clear
();
if
(
!
rallyPointsSupported
())
{
emit
loadComplete
(
QList
<
QGeoCoordinate
>
());
return
;
}
_cReadRallyPoints
=
_vehicle
->
parameterManager
()
->
getParameter
(
FactSystem
::
defaultComponentId
,
_rallyTotalParam
)
->
rawValue
().
toInt
();
qCDebug
(
RallyPointManagerLog
)
<<
"APMRallyPointManager::loadFromVehicle - point count"
<<
_cReadRallyPoints
;
if
(
_cReadRallyPoints
==
0
)
{
emit
loadComplete
(
_rgPoints
);
return
;
}
_currentRallyPoint
=
0
;
_readTransactionInProgress
=
true
;
_requestRallyPoint
(
_currentRallyPoint
);
}
/// Called when a new mavlink message for out vehicle is received
void
APMRallyPointManager
::
_mavlinkMessageReceived
(
const
mavlink_message_t
&
message
)
{
if
(
message
.
msgid
==
MAVLINK_MSG_ID_RALLY_POINT
)
{
mavlink_rally_point_t
rallyPoint
;
mavlink_msg_rally_point_decode
(
&
message
,
&
rallyPoint
);
qCDebug
(
RallyPointManagerLog
)
<<
"From vehicle rally_point: idx:lat:lng:alt"
<<
rallyPoint
.
idx
<<
rallyPoint
.
lat
<<
rallyPoint
.
lng
<<
rallyPoint
.
alt
;
if
(
rallyPoint
.
idx
!=
_currentRallyPoint
)
{
// FIXME: Protocol out of whack
_readTransactionInProgress
=
false
;
emit
inProgressChanged
(
inProgress
());
qCWarning
(
RallyPointManagerLog
)
<<
"Indices out of sync"
<<
rallyPoint
.
idx
<<
_currentRallyPoint
;
return
;
}
QGeoCoordinate
point
((
float
)
rallyPoint
.
lat
/
1e7
,
(
float
)
rallyPoint
.
lng
/
1e7
,
rallyPoint
.
alt
);
_rgPoints
.
append
(
point
);
if
(
rallyPoint
.
idx
<
_cReadRallyPoints
-
1
)
{
// Still more points to request
_requestRallyPoint
(
++
_currentRallyPoint
);
}
else
{
// We've finished collecting rally points
qCDebug
(
RallyPointManagerLog
)
<<
"Rally point load complete"
;
_readTransactionInProgress
=
false
;
emit
loadComplete
(
_rgPoints
);
emit
inProgressChanged
(
inProgress
());
}
}
}
void
APMRallyPointManager
::
_requestRallyPoint
(
uint8_t
pointIndex
)
{
mavlink_message_t
msg
;
MAVLinkProtocol
*
mavlink
=
qgcApp
()
->
toolbox
()
->
mavlinkProtocol
();
qCDebug
(
RallyPointManagerLog
)
<<
"APMRallyPointManager::_requestRallyPoint"
<<
pointIndex
;
mavlink_msg_rally_fetch_point_pack_chan
(
mavlink
->
getSystemId
(),
mavlink
->
getComponentId
(),
_vehicle
->
priorityLink
()
->
mavlinkChannel
(),
&
msg
,
_vehicle
->
id
(),
_vehicle
->
defaultComponentId
(),
pointIndex
);
_vehicle
->
sendMessageOnLink
(
_vehicle
->
priorityLink
(),
msg
);
}
void
APMRallyPointManager
::
_sendRallyPoint
(
uint8_t
pointIndex
)
{
mavlink_message_t
msg
;
MAVLinkProtocol
*
mavlink
=
qgcApp
()
->
toolbox
()
->
mavlinkProtocol
();
QGeoCoordinate
point
=
_rgPoints
[
pointIndex
];
mavlink_msg_rally_point_pack_chan
(
mavlink
->
getSystemId
(),
mavlink
->
getComponentId
(),
_vehicle
->
priorityLink
()
->
mavlinkChannel
(),
&
msg
,
_vehicle
->
id
(),
_vehicle
->
defaultComponentId
(),
pointIndex
,
_rgPoints
.
count
(),
point
.
latitude
()
*
1e7
,
point
.
longitude
()
*
1e7
,
point
.
altitude
(),
0
,
0
,
0
);
// break_alt, land_dir, flags
_vehicle
->
sendMessageOnLink
(
_vehicle
->
priorityLink
(),
msg
);
}
bool
APMRallyPointManager
::
inProgress
(
void
)
const
{
return
_readTransactionInProgress
||
_writeTransactionInProgress
;
}
bool
APMRallyPointManager
::
rallyPointsSupported
(
void
)
const
{
return
_vehicle
->
parameterManager
()
->
parameterExists
(
_vehicle
->
defaultComponentId
(),
_rallyTotalParam
);
}
void
APMRallyPointManager
::
removeAll
(
void
)
{
qCDebug
(
RallyPointManagerLog
)
<<
"APMRallyPointManager::removeAll"
;
QList
<
QGeoCoordinate
>
noRallyPoints
;
sendToVehicle
(
noRallyPoints
);
emit
removeAllComplete
(
false
/* error */
);
}
src/FirmwarePlugin/APM/APMRallyPointManager.h
deleted
100644 → 0
View file @
a11b1dbf
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*