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
d9fa1118
Commit
d9fa1118
authored
Aug 31, 2016
by
Don Gagne
Committed by
GitHub
Aug 31, 2016
Browse files
Options
Browse Files
Download
Plain Diff
Merge pull request #4005 from DonLakeFlyer/CircleFence
More work on GeoFence support
parents
bceb10ab
a75f4b6d
Changes
24
Expand all
Hide whitespace changes
Inline
Side-by-side
Showing
24 changed files
with
780 additions
and
443 deletions
+780
-443
qgroundcontrol.pro
qgroundcontrol.pro
+2
-0
AutoPilotPlugin.cc
src/AutoPilotPlugins/AutoPilotPlugin.cc
+1
-1
AutoPilotPlugin.h
src/AutoPilotPlugins/AutoPilotPlugin.h
+1
-1
APMFirmwarePlugin.h
src/FirmwarePlugin/APM/APMFirmwarePlugin.h
+20
-18
APMGeoFenceManager.cc
src/FirmwarePlugin/APM/APMGeoFenceManager.cc
+316
-0
APMGeoFenceManager.h
src/FirmwarePlugin/APM/APMGeoFenceManager.h
+73
-0
APMParameterFactMetaData.Copter.3.4.xml
...irmwarePlugin/APM/APMParameterFactMetaData.Copter.3.4.xml
+4
-0
ArduCopterFirmwarePlugin.cc
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
+12
-2
ArduCopterFirmwarePlugin.h
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
+4
-3
FirmwarePlugin.h
src/FirmwarePlugin/FirmwarePlugin.h
+11
-2
FlightDisplayViewMap.qml
src/FlightDisplay/FlightDisplayViewMap.qml
+16
-7
GeoFenceEditor.qml
src/MissionEditor/GeoFenceEditor.qml
+17
-2
MissionEditor.qml
src/MissionEditor/MissionEditor.qml
+40
-19
GeoFenceController.cc
src/MissionManager/GeoFenceController.cc
+118
-77
GeoFenceController.h
src/MissionManager/GeoFenceController.h
+36
-31
GeoFenceManager.cc
src/MissionManager/GeoFenceManager.cc
+16
-211
GeoFenceManager.h
src/MissionManager/GeoFenceManager.h
+36
-48
MissionController.cc
src/MissionManager/MissionController.cc
+38
-8
MissionController.h
src/MissionManager/MissionController.h
+6
-3
MissionManager.cc
src/MissionManager/MissionManager.cc
+1
-1
PlanElementController.cc
src/MissionManager/PlanElementController.cc
+3
-1
PlanElementController.h
src/MissionManager/PlanElementController.h
+2
-2
Vehicle.cc
src/Vehicle/Vehicle.cc
+6
-5
Vehicle.h
src/Vehicle/Vehicle.h
+1
-1
No files found.
qgroundcontrol.pro
View file @
d9fa1118
...
@@ -692,6 +692,7 @@ HEADERS+= \
...
@@ -692,6 +692,7 @@ HEADERS+= \
src/FirmwarePlugin/FirmwarePluginManager.h \
src/FirmwarePlugin/FirmwarePluginManager.h \
src/FirmwarePlugin/FirmwarePlugin.h \
src/FirmwarePlugin/FirmwarePlugin.h \
src/FirmwarePlugin/APM/APMFirmwarePlugin.h \
src/FirmwarePlugin/APM/APMFirmwarePlugin.h \
src/FirmwarePlugin/APM/APMGeoFenceManager.h \
src/FirmwarePlugin/APM/APMParameterMetaData.h \
src/FirmwarePlugin/APM/APMParameterMetaData.h \
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h \
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h \
src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h \
src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h \
...
@@ -751,6 +752,7 @@ SOURCES += \
...
@@ -751,6 +752,7 @@ SOURCES += \
src/AutoPilotPlugins/PX4/SensorsComponentController.cc \
src/AutoPilotPlugins/PX4/SensorsComponentController.cc \
src/AutoPilotPlugins/PX4/PX4TuningComponent.cc \
src/AutoPilotPlugins/PX4/PX4TuningComponent.cc \
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc \
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc \
src/FirmwarePlugin/APM/APMGeoFenceManager.cc \
src/FirmwarePlugin/APM/APMParameterMetaData.cc \
src/FirmwarePlugin/APM/APMParameterMetaData.cc \
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc \
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc \
src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc \
src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc \
...
...
src/AutoPilotPlugins/AutoPilotPlugin.cc
View file @
d9fa1118
...
@@ -117,7 +117,7 @@ void AutoPilotPlugin::refreshParametersPrefix(int componentId, const QString& na
...
@@ -117,7 +117,7 @@ void AutoPilotPlugin::refreshParametersPrefix(int componentId, const QString& na
_vehicle
->
getParameterLoader
()
->
refreshParametersPrefix
(
componentId
,
namePrefix
);
_vehicle
->
getParameterLoader
()
->
refreshParametersPrefix
(
componentId
,
namePrefix
);
}
}
bool
AutoPilotPlugin
::
parameterExists
(
int
componentId
,
const
QString
&
name
)
bool
AutoPilotPlugin
::
parameterExists
(
int
componentId
,
const
QString
&
name
)
const
{
{
return
_vehicle
->
getParameterLoader
()
->
parameterExists
(
componentId
,
name
);
return
_vehicle
->
getParameterLoader
()
->
parameterExists
(
componentId
,
name
);
}
}
...
...
src/AutoPilotPlugins/AutoPilotPlugin.h
View file @
d9fa1118
...
@@ -67,7 +67,7 @@
...
@@ -67,7 +67,7 @@
Q_INVOKABLE
void
refreshParametersPrefix
(
int
componentId
,
const
QString
&
namePrefix
);
Q_INVOKABLE
void
refreshParametersPrefix
(
int
componentId
,
const
QString
&
namePrefix
);
/// Returns true if the specifed parameter exists from the default component
/// Returns true if the specifed parameter exists from the default component
Q_INVOKABLE
bool
parameterExists
(
int
componentId
,
const
QString
&
name
);
Q_INVOKABLE
bool
parameterExists
(
int
componentId
,
const
QString
&
name
)
const
;
/// Returns all parameter names
/// Returns all parameter names
QStringList
parameterNames
(
int
componentId
);
QStringList
parameterNames
(
int
componentId
);
...
...
src/FirmwarePlugin/APM/APMFirmwarePlugin.h
View file @
d9fa1118
...
@@ -17,6 +17,7 @@
...
@@ -17,6 +17,7 @@
#include "FirmwarePlugin.h"
#include "FirmwarePlugin.h"
#include "QGCLoggingCategory.h"
#include "QGCLoggingCategory.h"
#include "APMParameterMetaData.h"
#include "APMParameterMetaData.h"
#include "APMGeoFenceManager.h"
#include <QAbstractSocket>
#include <QAbstractSocket>
...
@@ -73,24 +74,25 @@ public:
...
@@ -73,24 +74,25 @@ public:
QList
<
VehicleComponent
*>
componentsForVehicle
(
AutoPilotPlugin
*
vehicle
)
final
;
QList
<
VehicleComponent
*>
componentsForVehicle
(
AutoPilotPlugin
*
vehicle
)
final
;
QList
<
MAV_CMD
>
supportedMissionCommands
(
void
)
final
;
QList
<
MAV_CMD
>
supportedMissionCommands
(
void
)
final
;
bool
isCapable
(
const
Vehicle
*
vehicle
,
FirmwareCapabilities
capabilities
);
bool
isCapable
(
const
Vehicle
*
vehicle
,
FirmwareCapabilities
capabilities
);
QStringList
flightModes
(
Vehicle
*
vehicle
)
final
;
QStringList
flightModes
(
Vehicle
*
vehicle
)
final
;
QString
flightMode
(
uint8_t
base_mode
,
uint32_t
custom_mode
)
const
final
;
QString
flightMode
(
uint8_t
base_mode
,
uint32_t
custom_mode
)
const
final
;
bool
setFlightMode
(
const
QString
&
flightMode
,
uint8_t
*
base_mode
,
uint32_t
*
custom_mode
)
final
;
bool
setFlightMode
(
const
QString
&
flightMode
,
uint8_t
*
base_mode
,
uint32_t
*
custom_mode
)
final
;
bool
isGuidedMode
(
const
Vehicle
*
vehicle
)
const
final
;
bool
isGuidedMode
(
const
Vehicle
*
vehicle
)
const
final
;
void
pauseVehicle
(
Vehicle
*
vehicle
);
void
pauseVehicle
(
Vehicle
*
vehicle
);
int
manualControlReservedButtonCount
(
void
);
int
manualControlReservedButtonCount
(
void
);
bool
adjustIncomingMavlinkMessage
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
)
final
;
bool
adjustIncomingMavlinkMessage
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
)
final
;
void
adjustOutgoingMavlinkMessage
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
)
final
;
void
adjustOutgoingMavlinkMessage
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
)
final
;
void
initializeVehicle
(
Vehicle
*
vehicle
)
final
;
void
initializeVehicle
(
Vehicle
*
vehicle
)
final
;
bool
sendHomePositionToVehicle
(
void
)
final
;
bool
sendHomePositionToVehicle
(
void
)
final
;
void
addMetaDataToFact
(
QObject
*
parameterMetaData
,
Fact
*
fact
,
MAV_TYPE
vehicleType
)
final
;
void
addMetaDataToFact
(
QObject
*
parameterMetaData
,
Fact
*
fact
,
MAV_TYPE
vehicleType
)
final
;
QString
getDefaultComponentIdParam
(
void
)
const
final
{
return
QString
(
"SYSID_SW_TYPE"
);
}
QString
getDefaultComponentIdParam
(
void
)
const
final
{
return
QString
(
"SYSID_SW_TYPE"
);
}
QString
missionCommandOverrides
(
MAV_TYPE
vehicleType
)
const
;
QString
missionCommandOverrides
(
MAV_TYPE
vehicleType
)
const
;
QString
getVersionParam
(
void
)
final
{
return
QStringLiteral
(
"SYSID_SW_MREV"
);
}
QString
getVersionParam
(
void
)
final
{
return
QStringLiteral
(
"SYSID_SW_MREV"
);
}
QString
internalParameterMetaDataFile
(
void
)
final
{
return
QString
(
":/FirmwarePlugin/APM/APMParameterFactMetaData.xml"
);
}
QString
internalParameterMetaDataFile
(
void
)
final
{
return
QString
(
":/FirmwarePlugin/APM/APMParameterFactMetaData.xml"
);
}
void
getParameterMetaDataVersionInfo
(
const
QString
&
metaDataFile
,
int
&
majorVersion
,
int
&
minorVersion
)
final
{
APMParameterMetaData
::
getParameterMetaDataVersionInfo
(
metaDataFile
,
majorVersion
,
minorVersion
);
}
void
getParameterMetaDataVersionInfo
(
const
QString
&
metaDataFile
,
int
&
majorVersion
,
int
&
minorVersion
)
final
{
APMParameterMetaData
::
getParameterMetaDataVersionInfo
(
metaDataFile
,
majorVersion
,
minorVersion
);
}
QObject
*
loadParameterMetaData
(
const
QString
&
metaDataFile
);
QObject
*
loadParameterMetaData
(
const
QString
&
metaDataFile
);
GeoFenceManager
*
newGeoFenceManager
(
Vehicle
*
vehicle
)
{
return
new
APMGeoFenceManager
(
vehicle
);
}
QString
getParameterMetaDataFile
(
Vehicle
*
vehicle
);
QString
getParameterMetaDataFile
(
Vehicle
*
vehicle
);
...
...
src/FirmwarePlugin/APM/APMGeoFenceManager.cc
0 → 100644
View file @
d9fa1118
This diff is collapsed.
Click to expand it.
src/FirmwarePlugin/APM/APMGeoFenceManager.h
0 → 100644
View file @
d9fa1118
/****************************************************************************
*
* (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
APMGeoFenceManager
:
public
GeoFenceManager
{
Q_OBJECT
public:
APMGeoFenceManager
(
Vehicle
*
vehicle
);
~
APMGeoFenceManager
();
// Overrides from GeoFenceManager
bool
inProgress
(
void
)
const
final
;
void
loadFromVehicle
(
void
)
final
;
void
sendToVehicle
(
void
)
final
;
bool
fenceSupported
(
void
)
const
final
{
return
_fenceSupported
;
}
bool
circleSupported
(
void
)
const
final
;
bool
polygonSupported
(
void
)
const
final
;
bool
breachReturnSupported
(
void
)
const
final
{
return
_breachReturnSupported
;
}
float
circleRadius
(
void
)
const
final
;
QVariantList
params
(
void
)
const
final
{
return
_params
;
}
QStringList
paramLabels
(
void
)
const
final
{
return
_paramLabels
;
}
private
slots
:
void
_mavlinkMessageReceived
(
const
mavlink_message_t
&
message
);
void
_updateSupportedFlags
(
void
);
void
_circleRadiusRawValueChanged
(
QVariant
value
);
void
_parametersReady
(
void
);
private:
void
_requestFencePoint
(
uint8_t
pointIndex
);
void
_sendFencePoint
(
uint8_t
pointIndex
);
bool
_geoFenceSupported
(
void
);
private:
bool
_fenceSupported
;
bool
_breachReturnSupported
;
bool
_firstParamLoadComplete
;
QVariantList
_params
;
QStringList
_paramLabels
;
Fact
*
_circleRadiusFact
;
bool
_readTransactionInProgress
;
bool
_writeTransactionInProgress
;
uint8_t
_cReadFencePoints
;
uint8_t
_cWriteFencePoints
;
uint8_t
_currentFencePoint
;
Fact
*
_fenceEnableFact
;
Fact
*
_fenceTypeFact
;
static
const
char
*
_fenceTotalParam
;
static
const
char
*
_fenceActionParam
;
static
const
char
*
_fenceEnableParam
;
};
#endif
src/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.4.xml
View file @
d9fa1118
...
@@ -6512,6 +6512,10 @@
...
@@ -6512,6 +6512,10 @@
<value
code=
"1"
>
Altitude
</value>
<value
code=
"1"
>
Altitude
</value>
<value
code=
"2"
>
Circle
</value>
<value
code=
"2"
>
Circle
</value>
<value
code=
"3"
>
Altitude and Circle
</value>
<value
code=
"3"
>
Altitude and Circle
</value>
<value
code=
"4"
>
Polygon
</value>
<value
code=
"5"
>
Altitude and Polygon
</value>
<value
code=
"6"
>
Circle and Polygon
</value>
<value
code=
"7"
>
All
</value>
</values>
</values>
</param>
</param>
<param
humanName=
"Fence Action"
name=
"FENCE_ACTION"
documentation=
"What action should be taken when fence is breached"
user=
"Standard"
>
<param
humanName=
"Fence Action"
name=
"FENCE_ACTION"
documentation=
"What action should be taken when fence is breached"
user=
"Standard"
>
...
...
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
View file @
d9fa1118
...
@@ -98,9 +98,13 @@ int ArduCopterFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVe
...
@@ -98,9 +98,13 @@ int ArduCopterFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVe
return
majorVersionNumber
==
3
?
4
:
Vehicle
::
versionNotSetValue
;
return
majorVersionNumber
==
3
?
4
:
Vehicle
::
versionNotSetValue
;
}
}
bool
ArduCopterFirmwarePlugin
::
isCapable
(
const
Vehicle
*
/*vehicle*/
,
FirmwareCapabilities
capabilities
)
bool
ArduCopterFirmwarePlugin
::
isCapable
(
const
Vehicle
*
vehicle
,
FirmwareCapabilities
capabilities
)
{
{
return
(
capabilities
&
(
SetFlightModeCapability
|
GuidedModeCapability
|
PauseVehicleCapability
))
==
capabilities
;
Q_UNUSED
(
vehicle
);
uint32_t
vehicleCapabilities
=
SetFlightModeCapability
|
GuidedModeCapability
|
PauseVehicleCapability
;
return
(
capabilities
&
vehicleCapabilities
)
==
capabilities
;
}
}
void
ArduCopterFirmwarePlugin
::
guidedModeRTL
(
Vehicle
*
vehicle
)
void
ArduCopterFirmwarePlugin
::
guidedModeRTL
(
Vehicle
*
vehicle
)
...
@@ -208,3 +212,9 @@ bool ArduCopterFirmwarePlugin::multiRotorXConfig(Vehicle* vehicle)
...
@@ -208,3 +212,9 @@ bool ArduCopterFirmwarePlugin::multiRotorXConfig(Vehicle* vehicle)
{
{
return
vehicle
->
autopilotPlugin
()
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
"FRAME"
)
->
rawValue
().
toInt
()
!=
0
;
return
vehicle
->
autopilotPlugin
()
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
"FRAME"
)
->
rawValue
().
toInt
()
!=
0
;
}
}
QString
ArduCopterFirmwarePlugin
::
geoFenceRadiusParam
(
Vehicle
*
vehicle
)
{
Q_UNUSED
(
vehicle
);
return
QStringLiteral
(
"FENCE_RADIUS"
);
}
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
View file @
d9fa1118
...
@@ -63,9 +63,10 @@ public:
...
@@ -63,9 +63,10 @@ public:
void
guidedModeGotoLocation
(
Vehicle
*
vehicle
,
const
QGeoCoordinate
&
gotoCoord
)
final
;
void
guidedModeGotoLocation
(
Vehicle
*
vehicle
,
const
QGeoCoordinate
&
gotoCoord
)
final
;
void
guidedModeChangeAltitude
(
Vehicle
*
vehicle
,
double
altitudeRel
)
final
;
void
guidedModeChangeAltitude
(
Vehicle
*
vehicle
,
double
altitudeRel
)
final
;
const
FirmwarePlugin
::
remapParamNameMajorVersionMap_t
&
paramNameRemapMajorVersionMap
(
void
)
const
final
{
return
_remapParamName
;
}
const
FirmwarePlugin
::
remapParamNameMajorVersionMap_t
&
paramNameRemapMajorVersionMap
(
void
)
const
final
{
return
_remapParamName
;
}
virtual
int
remapParamNameHigestMinorVersionNumber
(
int
majorVersionNumber
)
const
final
;
int
remapParamNameHigestMinorVersionNumber
(
int
majorVersionNumber
)
const
final
;
virtual
bool
multiRotorCoaxialMotors
(
Vehicle
*
vehicle
)
final
;
bool
multiRotorCoaxialMotors
(
Vehicle
*
vehicle
)
final
;
virtual
bool
multiRotorXConfig
(
Vehicle
*
vehicle
)
final
;
bool
multiRotorXConfig
(
Vehicle
*
vehicle
)
final
;
QString
geoFenceRadiusParam
(
Vehicle
*
vehicle
)
final
;
private:
private:
static
bool
_remapParamNameIntialized
;
static
bool
_remapParamNameIntialized
;
...
...
src/FirmwarePlugin/FirmwarePlugin.h
View file @
d9fa1118
...
@@ -17,6 +17,7 @@
...
@@ -17,6 +17,7 @@
#include "QGCMAVLink.h"
#include "QGCMAVLink.h"
#include "VehicleComponent.h"
#include "VehicleComponent.h"
#include "AutoPilotPlugin.h"
#include "AutoPilotPlugin.h"
#include "GeoFenceManager.h"
#include <QList>
#include <QList>
#include <QString>
#include <QString>
...
@@ -40,8 +41,8 @@ public:
...
@@ -40,8 +41,8 @@ public:
SetFlightModeCapability
=
1
<<
0
,
///< FirmwarePlugin::setFlightMode method is supported
SetFlightModeCapability
=
1
<<
0
,
///< FirmwarePlugin::setFlightMode method is supported
MavCmdPreflightStorageCapability
=
1
<<
1
,
///< MAV_CMD_PREFLIGHT_STORAGE is supported
MavCmdPreflightStorageCapability
=
1
<<
1
,
///< MAV_CMD_PREFLIGHT_STORAGE is supported
PauseVehicleCapability
=
1
<<
2
,
///< Vehicle supports pausing at current location
PauseVehicleCapability
=
1
<<
2
,
///< Vehicle supports pausing at current location
GuidedModeCapability
=
1
<<
3
,
///< Vehicle
S
upports guided mode commands
GuidedModeCapability
=
1
<<
3
,
///< Vehicle
s
upports guided mode commands
OrbitModeCapability
=
1
<<
4
,
///< Vehicle
S
upports orbit mode
OrbitModeCapability
=
1
<<
4
,
///< Vehicle
s
upports orbit mode
}
FirmwareCapabilities
;
}
FirmwareCapabilities
;
/// Maps from on parameter name to another
/// Maps from on parameter name to another
...
@@ -207,6 +208,14 @@ public:
...
@@ -207,6 +208,14 @@ public:
/// @return true: X confiuration, false: Plus configuration
/// @return true: X confiuration, false: Plus configuration
virtual
bool
multiRotorXConfig
(
Vehicle
*
vehicle
)
{
Q_UNUSED
(
vehicle
);
return
false
;
}
virtual
bool
multiRotorXConfig
(
Vehicle
*
vehicle
)
{
Q_UNUSED
(
vehicle
);
return
false
;
}
/// Returns a newly create geofence manager for this vehicle. Returns NULL if this vehicle
/// does not support geofence. You should make sense to check vehicle capabilites for geofence
/// support.
virtual
GeoFenceManager
*
newGeoFenceManager
(
Vehicle
*
vehicle
)
{
return
new
GeoFenceManager
(
vehicle
);
}
/// Returns the parameter which holds the fence circle radius if supported.
virtual
QString
geoFenceRadiusParam
(
Vehicle
*
vehicle
)
{
Q_UNUSED
(
vehicle
);
return
QString
();
}
};
};
#endif
#endif
src/FlightDisplay/FlightDisplayViewMap.qml
View file @
d9fa1118
...
@@ -27,7 +27,7 @@ FlightMap {
...
@@ -27,7 +27,7 @@ FlightMap {
anchors.fill
:
parent
anchors.fill
:
parent
mapName
:
_mapName
mapName
:
_mapName
property
alias
missionController
:
_
missionController
property
alias
missionController
:
missionController
property
var
flightWidgets
property
var
flightWidgets
property
bool
_followVehicle
:
true
property
bool
_followVehicle
:
true
...
@@ -54,12 +54,12 @@ FlightMap {
...
@@ -54,12 +54,12 @@ FlightMap {
QGCPalette
{
id
:
qgcPal
;
colorGroupEnabled
:
true
}
QGCPalette
{
id
:
qgcPal
;
colorGroupEnabled
:
true
}
MissionController
{
MissionController
{
id
:
_
missionController
id
:
missionController
Component.onCompleted
:
start
(
false
/* editMode */
)
Component.onCompleted
:
start
(
false
/* editMode */
)
}
}
GeoFenceController
{
GeoFenceController
{
id
:
_
geoFenceController
id
:
geoFenceController
Component.onCompleted
:
start
(
false
/* editMode */
)
Component.onCompleted
:
start
(
false
/* editMode */
)
}
}
...
@@ -93,25 +93,34 @@ FlightMap {
...
@@ -93,25 +93,34 @@ FlightMap {
// Add the mission items to the map
// Add the mission items to the map
MissionItemView
{
MissionItemView
{
model
:
_mainIsMap
?
_
missionController
.
visualItems
:
0
model
:
_mainIsMap
?
missionController
.
visualItems
:
0
}
}
// Add lines between waypoints
// Add lines between waypoints
MissionLineView
{
MissionLineView
{
model
:
_mainIsMap
?
_
missionController
.
waypointLines
:
0
model
:
_mainIsMap
?
missionController
.
waypointLines
:
0
}
}
// GeoFence polygon
// GeoFence polygon
MapPolygon
{
MapPolygon
{
border.color
:
"
#80FF0000
"
border.color
:
"
#80FF0000
"
border.width
:
3
border.width
:
3
path
:
_geoFenceController
.
polygon
.
path
path
:
geoFenceController
.
polygonSupported
?
geoFenceController
.
polygon
.
path
:
undefined
}
// GeoFence circle
MapCircle
{
border.color
:
"
#80FF0000
"
border.width
:
3
center
:
missionController
.
plannedHomePosition
radius
:
geoFenceController
.
circleSupported
?
geoFenceController
.
circleRadius
:
0
}
}
// GeoFence breach return point
// GeoFence breach return point
MapQuickItem
{
MapQuickItem
{
anchorPoint
:
Qt
.
point
(
sourceItem
.
width
/
2
,
sourceItem
.
height
/
2
)
anchorPoint
:
Qt
.
point
(
sourceItem
.
width
/
2
,
sourceItem
.
height
/
2
)
coordinate
:
_geoFenceController
.
breachReturnPoint
coordinate
:
geoFenceController
.
breachReturnPoint
visible
:
geoFenceController
.
breachReturnSupported
sourceItem
:
MissionItemIndexLabel
{
label
:
"
F
"
}
sourceItem
:
MissionItemIndexLabel
{
label
:
"
F
"
}
}
}
...
...
src/MissionEditor/GeoFenceEditor.qml
View file @
d9fa1118
...
@@ -7,6 +7,7 @@ import QGroundControl.Controls 1.0
...
@@ -7,6 +7,7 @@ import QGroundControl.Controls 1.0
import
QGroundControl
.
FactControls
1.0
import
QGroundControl
.
FactControls
1.0
import
QGroundControl
.
Palette
1.0
import
QGroundControl
.
Palette
1.0
import
QGroundControl
.
FlightMap
1.0
import
QGroundControl
.
FlightMap
1.0
import
QGroundControl
.
FactSystem
1.0
QGCFlickable
{
QGCFlickable
{
id
:
root
id
:
root
...
@@ -15,7 +16,7 @@ QGCFlickable {
...
@@ -15,7 +16,7 @@ QGCFlickable {
contentHeight
:
geoFenceEditorRect
.
height
contentHeight
:
geoFenceEditorRect
.
height
clip
:
true
clip
:
true
readonly
property
real
_editFieldWidth
:
Math
.
min
(
width
-
_margin
*
2
,
ScreenTools
.
defaultFontPixelWidth
*
1
2
)
readonly
property
real
_editFieldWidth
:
Math
.
min
(
width
-
_margin
*
2
,
ScreenTools
.
defaultFontPixelWidth
*
1
5
)
readonly
property
real
_margin
:
ScreenTools
.
defaultFontPixelWidth
/
2
readonly
property
real
_margin
:
ScreenTools
.
defaultFontPixelWidth
/
2
readonly
property
real
_radius
:
ScreenTools
.
defaultFontPixelWidth
/
2
readonly
property
real
_radius
:
ScreenTools
.
defaultFontPixelWidth
/
2
...
@@ -70,6 +71,7 @@ QGCFlickable {
...
@@ -70,6 +71,7 @@ QGCFlickable {
anchors.right
:
parent
.
right
anchors.right
:
parent
.
right
wrapMode
:
Text
.
WordWrap
wrapMode
:
Text
.
WordWrap
text
:
qsTr
(
"
Click in map to set breach return point.
"
)
text
:
qsTr
(
"
Click in map to set breach return point.
"
)
visible
:
geoFenceController
.
breachReturnSupported
}
}
QGCLabel
{
text
:
qsTr
(
"
Fence Settings:
"
)
}
QGCLabel
{
text
:
qsTr
(
"
Fence Settings:
"
)
}
...
@@ -96,7 +98,7 @@ QGCFlickable {
...
@@ -96,7 +98,7 @@ QGCFlickable {
QGCLabel
{
QGCLabel
{
id
:
textFieldLabel
id
:
textFieldLabel
anchors.baseline
:
textField
.
baseline
anchors.baseline
:
textField
.
baseline
text
:
modelData
.
name
text
:
geoFenceController
.
paramLabels
[
index
]
}
}
FactTextField
{
FactTextField
{
...
@@ -105,6 +107,18 @@ QGCFlickable {
...
@@ -105,6 +107,18 @@ QGCFlickable {
width
:
_editFieldWidth
width
:
_editFieldWidth
showUnits
:
true
showUnits
:
true
fact
:
modelData
fact
:
modelData
visible
:
!
comboField
.
visible
}
FactComboBox
{
id
:
comboField
anchors.right
:
parent
.
right
width
:
_editFieldWidth
indexModel
:
false
fact
:
visible
?
modelData
:
_nullFact
visible
:
modelData
.
enumStrings
.
length
property
var
_nullFact
:
Fact
{
}
}
}
}
}
}
}
...
@@ -115,6 +129,7 @@ QGCFlickable {
...
@@ -115,6 +129,7 @@ QGCFlickable {
flightMap
:
editorMap
flightMap
:
editorMap
polygon
:
root
.
polygon
polygon
:
root
.
polygon
sectionLabel
:
qsTr
(
"
Fence Polygon:
"
)
sectionLabel
:
qsTr
(
"
Fence Polygon:
"
)
visible
:
geoFenceController
.
polygonSupported
}
}
}
}
}
}
...
...
src/MissionEditor/MissionEditor.qml
View file @
d9fa1118
...
@@ -154,6 +154,12 @@ QGCView {
...
@@ -154,6 +154,12 @@ QGCView {
id
:
geoFenceController
id
:
geoFenceController
Component.onCompleted
:
start
(
true
/* editMode */
)
Component.onCompleted
:
start
(
true
/* editMode */
)
onFenceSupportedChanged
:
{
if
(
!
fenceSupported
&&
_editingLayer
==
_layerGeoFence
)
{
_editingLayer
=
_layerMission
}
}
}
}
QGCPalette
{
id
:
qgcPal
;
colorGroupEnabled
:
enabled
}
QGCPalette
{
id
:
qgcPal
;
colorGroupEnabled
:
enabled
}
...
@@ -515,7 +521,7 @@ QGCView {
...
@@ -515,7 +521,7 @@ QGCView {
setCurrentItem
(
sequenceNumber
)
setCurrentItem
(
sequenceNumber
)
}
}
onMoveHomeToMapCenter
:
missionController
.
visualItems
.
get
(
0
).
coordinate
=
editorMap
.
center
onMoveHomeToMapCenter
:
_
visualItems
.
get
(
0
).
coordinate
=
editorMap
.
center
}
}
}
// ListView
}
// ListView
}
// Item - Mission Item editor
}
// Item - Mission Item editor
...
@@ -537,16 +543,31 @@ QGCView {
...
@@ -537,16 +543,31 @@ QGCView {
MapPolygon
{
MapPolygon
{
border.color
:
"
#80FF0000
"
border.color
:
"
#80FF0000
"
border.width
:
3
border.width
:
3
path
:
geoFenceController
.
polygon
.
path
path
:
geoFenceController
.
polygonSupported
?
geoFenceController
.
polygon
.
path
:
undefined
}
// GeoFence circle
MapCircle
{
border.color
:
"
#80FF0000
"
border.width
:
3
center
:
missionController
.
plannedHomePosition
radius
:
geoFenceController
.
circleSupported
?
geoFenceController
.
circleRadius
:
0
}
// GeoFence circle
MapCircle
{
border.color
:
"
#80FF0000
"
border.width
:
3
center
:
missionController
.
plannedHomePosition
radius
:
geoFenceController
.
circleSupported
?
geoFenceController
.
circleRadius
:
0
}
}
// GeoFence breach return point
// GeoFence breach return point
MapQuickItem
{
MapQuickItem
{
anchorPoint
:
Qt
.
point
(
sourceItem
.
width
/
2
,
sourceItem
.
height
/
2
)
anchorPoint
:
Qt
.
point
(
sourceItem
.
width
/
2
,
sourceItem
.
height
/
2
)
coordinate
:
geoFenceController
.
breachReturnPoint
coordinate
:
geoFenceController
.
breachReturnPoint
sourceItem
:
MissionItemIndexLabel
{
visible
:
geoFenceController
.
breachReturnSupported
label
:
"
F
"
sourceItem
:
MissionItemIndexLabel
{
label
:
"
F
"
}
}
}
}
//-- Dismiss Drop Down (if any)
//-- Dismiss Drop Down (if any)
...
@@ -583,10 +604,10 @@ QGCView {
...
@@ -583,10 +604,10 @@ QGCView {
DropButton
{
DropButton
{
id
:
layerButton
id
:
layerButton
dropDirection
:
dropRight
dropDirection
:
dropRight
//buttonImage: "/qmlimages/MapCenter.svg"
viewportMargins
:
ScreenTools
.
defaultFontPixelWidth
/
2
viewportMargins
:
ScreenTools
.
defaultFontPixelWidth
/
2
exclusiveGroup
:
_dropButtonsExclusiveGroup
exclusiveGroup
:
_dropButtonsExclusiveGroup
lightBorders
:
_lightWidgetBorders
lightBorders
:
_lightWidgetBorders
visible
:
geoFenceController
.
fenceSupported
dropDownComponent
:
Component
{
dropDownComponent
:
Component
{
Column
{
Column
{
...
@@ -775,19 +796,19 @@ QGCView {
...
@@ -775,19 +796,19 @@ QGCView {
}
}
MissionItemStatus
{
MissionItemStatus
{
id
:
waypointValuesDisplay
id
:
waypointValuesDisplay
anchors.margins
:
ScreenTools
.
defaultFontPixelWidth
anchors.margins
:
ScreenTools
.
defaultFontPixelWidth
anchors.left
:
parent
.
left
anchors.left
:
parent
.
left
anchors.bottom
:
parent
.
bottom
anchors.bottom
:
parent
.
bottom
z
:
QGroundControl
.
zOrderTopMost
z
:
QGroundControl
.
zOrderTopMost
currentMissionItem
:
_currentMissionItem
currentMissionItem
:
_currentMissionItem
missionItems
:
missionController
.
visualItems
missionItems
:
missionController
.
visualItems
expandedWidth
:
missionItemEditor
.
x
-
(
ScreenTools
.
defaultFontPixelWidth
*
2
)
expandedWidth
:
missionItemEditor
.
x
-
(
ScreenTools
.
defaultFontPixelWidth
*
2
)
missionDistance
:
missionController
.
missionDistance
missionDistance
:
missionController
.
missionDistance
missionMaxTelemetry
:
missionController
.
missionMaxTelemetry
missionMaxTelemetry
:
missionController
.
missionMaxTelemetry
cruiseDistance
:
missionController
.
cruiseDistance
cruiseDistance
:
missionController
.
cruiseDistance
hoverDistance
:
missionController
.
hoverDistance
hoverDistance
:
missionController
.
hoverDistance
visible
:
_editingLayer
==
_layerMission
&&
!
ScreenTools
.
isShortScreen
visible
:
_editingLayer
==
_layerMission
&&
!
ScreenTools
.
isShortScreen
}
}
}
// FlightMap
}
// FlightMap
}
// Item - split view container
}
// Item - split view container
...
...
src/MissionManager/GeoFenceController.cc
View file @
d9fa1118
This diff is collapsed.
Click to expand it.
src/MissionManager/GeoFenceController.h
View file @
d9fa1118
...
@@ -27,17 +27,15 @@ public:
...
@@ -27,17 +27,15 @@ public:
GeoFenceController
(
QObject
*
parent
=
NULL
);
GeoFenceController
(
QObject
*
parent
=
NULL
);
~
GeoFenceController
();
~
GeoFenceController
();
enum
GeoFenceTypeEnum
{
Q_PROPERTY
(
bool
fenceSupported
READ
fenceSupported
NOTIFY
fenceSupportedChanged
)
GeoFenceNone
=
GeoFenceManager
::
GeoFenceNone
,
Q_PROPERTY
(
bool
circleSupported
READ
circleSupported
NOTIFY
circleSupportedChanged
)
GeoFenceCircle
=
GeoFenceManager
::
GeoFenceCircle
,
Q_PROPERTY
(
bool
polygonSupported
READ
polygonSupported
NOTIFY
polygonSupportedChanged
)
GeoFencePolygon
=
GeoFenceManager
::
GeoFencePolygon
,
Q_PROPERTY
(
bool
breachReturnSupported
READ
breachReturnSupported
NOTIFY
breachReturnSupportedChanged
)
};
Q_PROPERTY
(
float
circleRadius
READ
circleRadius
NOTIFY
circleRadiusChanged
)
Q_PROPERTY
(
QGCMapPolygon
*
polygon
READ
polygon
CONSTANT
)
Q_PROPERTY
(
GeoFenceTypeEnum
fenceType
READ
fenceType
WRITE
setFenceType
NOTIFY
fenceTypeChanged
)
Q_PROPERTY
(
QGeoCoordinate
breachReturnPoint
READ
breachReturnPoint
WRITE
setBreachReturnPoint
NOTIFY
breachReturnPointChanged
)
Q_PROPERTY
(
float
circleRadius
READ
circleRadius
WRITE
setCircleRadius
NOTIFY
circleRadiusChanged
)
Q_PROPERTY
(
QVariantList
params
READ
params
NOTIFY
paramsChanged
)
Q_PROPERTY
(
QGCMapPolygon
*
polygon
READ
polygon
CONSTANT
)
Q_PROPERTY
(
QStringList
paramLabels
READ
paramLabels
NOTIFY
paramLabelsChanged
)
Q_PROPERTY
(
QGeoCoordinate
breachReturnPoint
READ
breachReturnPoint
WRITE
setBreachReturnPoint
NOTIFY
breachReturnPointChanged
)
Q_PROPERTY
(
QVariantList
params
READ
params
NOTIFY
paramsChanged
)
void
start
(
bool
editMode
)
final
;
void
start
(
bool
editMode
)
final
;
void
loadFromVehicle
(
void
)
final
;
void
loadFromVehicle
(
void
)
final
;
...
@@ -51,39 +49,46 @@ public:
...
@@ -51,39 +49,46 @@ public:
bool
dirty
(
void
)
const
final
;
bool
dirty
(
void
)
const
final
;
void
setDirty
(
bool
dirty
)
final
;
void
setDirty
(
bool
dirty
)
final
;
GeoFenceTypeEnum
fenceType
(
void
)
const
{
return
(
GeoFenceTypeEnum
)
_geoFence
.
fenceType
;
}
bool
fenceSupported
(
void
)
const
;
float
circleRadius
(
void
)
const
{
return
_geoFence
.
circleRadius
;
}
bool
circleSupported
(
void
)
const
;
QGCMapPolygon
*
polygon
(
void
)
{
return
&
_geoFence
.
polygon
;
}
bool
polygonSupported
(
void
)
const
;
QGeoCoordinate
breachReturnPoint
(
void
)
const
{
return
_geoFence
.
breachReturnPoint
;
}
bool
breachReturnSupported
(
void
)
const
;
QVariantList
params
(
void
)
{
return
_params
;
}
float
circleRadius
(
void
)
const
;
QGCMapPolygon
*
polygon
(
void
)
{
return
&
_polygon
;
}
QGeoCoordinate
breachReturnPoint
(
void
)
const
{
return
_breachReturnPoint
;
}
QVariantList
params
(
void
)
const
;
QStringList
paramLabels
(
void
)
const
;
void
setFenceType
(
GeoFenceTypeEnum
fenceType
);
public
slots
:
void
setCircleRadius
(
float
circleRadius
);
void
setBreachReturnPoint
(
const
QGeoCoordinate
&
breachReturnPoint
);
void
setBreachReturnPoint
(
const
QGeoCoordinate
&
breachReturnPoint
);
signals:
signals:
void
fenceTypeChanged
(
GeoFenceTypeEnum
fenceType
);
void
fenceSupportedChanged
(
bool
fenceSupported
);
void
circleRadiusChanged
(
float
circleRadius
);
void
circleSupportedChanged
(
bool
circleSupported
);
void
polygonPathChanged
(
const
QVariantList
&
polygonPath
);
void
polygonSupportedChanged
(
bool
polygonSupported
);
void
breachReturnPointChanged
(
QGeoCoordinate
breachReturnPoint
);
void
breachReturnSupportedChanged
(
bool
breachReturnSupported
);
void
paramsChanged
(
void
);
void
circleRadiusChanged
(
float
circleRadius
);
void
polygonPathChanged
(
const
QVariantList
&
polygonPath
);
void
breachReturnPointChanged
(
QGeoCoordinate
breachReturnPoint
);
void
paramsChanged
(
QVariantList
params
);
void
paramLabelsChanged
(
QStringList
paramLabels
);
private
slots
:
private
slots
:
void
_parameterReadyVehicleAvailableChanged
(
bool
parameterReadyVehicleAvailable
);
void
_newGeoFenceAvailable
(
void
);
void
_polygonDirtyChanged
(
bool
dirty
);
void
_polygonDirtyChanged
(
bool
dirty
);
void
_setDirty
(
void
);
void
_setPolygon
(
const
QList
<
QGeoCoordinate
>&
polygon
);
private:
private:
void
_setParams
(
void
);
void
_clearGeoFence
(
void
);
void
_clearGeoFence
(
void
);
void
_s
etGeoFence
(
const
GeoFenceManager
::
GeoFence_t
&
geoFence
);
void
_s
ignalAll
(
void
);
void
_activeVehicleBeingRemoved
(
void
)
final
;
void
_activeVehicleBeingRemoved
(
Vehicle
*
vehicle
)
final
;
void
_activeVehicleSet
(
void
)
final
;
void
_activeVehicleSet
(
void
)
final
;
bool
_dirty
;
bool
_dirty
;
GeoFenceManager
::
GeoFence_t
_geoFence
;
QGCMapPolygon
_polygon
;
QVariantList
_params
;
QGeoCoordinate
_breachReturnPoint
;
QVariantList
_params
;
};
};
#endif
#endif
src/MissionManager/GeoFenceManager.cc
View file @
d9fa1118
...
@@ -7,27 +7,16 @@
...
@@ -7,27 +7,16 @@
*
*
****************************************************************************/
****************************************************************************/
/// @file
/// @author Don Gagne <don@thegagnes.com>
#include "GeoFenceManager.h"
#include "GeoFenceManager.h"
#include "Vehicle.h"
#include "Vehicle.h"
#include "FirmwarePlugin.h"
#include "MAVLinkProtocol.h"
#include "QGCApplication.h"
QGC_LOGGING_CATEGORY
(
GeoFenceManagerLog
,
"GeoFenceManagerLog"
)
QGC_LOGGING_CATEGORY
(
GeoFenceManagerLog
,
"GeoFenceManagerLog"
)
const
char
*
GeoFenceManager
::
_fenceTotalParam
=
"FENCE_TOTAL"
;
const
char
*
GeoFenceManager
::
_fenceActionParam
=
"FENCE_ACTION"
;
GeoFenceManager
::
GeoFenceManager
(
Vehicle
*
vehicle
)
GeoFenceManager
::
GeoFenceManager
(
Vehicle
*
vehicle
)
:
_vehicle
(
vehicle
)
:
QObject
(
vehicle
)
,
_readTransactionInProgress
(
false
)
,
_vehicle
(
vehicle
)
,
_writeTransactionInProgress
(
false
)
{
{
connect
(
_vehicle
,
&
Vehicle
::
mavlinkMessageReceived
,
this
,
&
GeoFenceManager
::
_mavlinkMessageReceived
);
}
}
GeoFenceManager
::~
GeoFenceManager
()
GeoFenceManager
::~
GeoFenceManager
()
...
@@ -42,213 +31,29 @@ void GeoFenceManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg)
...
@@ -42,213 +31,29 @@ void GeoFenceManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg)
emit
error
(
errorCode
,
errorMsg
);
emit
error
(
errorCode
,
errorMsg
);
}
}
void
GeoFenceManager
::
setGeoFence
(
const
GeoFence_t
&
geoFence
)
void
GeoFenceManager
::
loadFromVehicle
(
void
)
{
{
if
(
_readTransactionInProgress
)
{
// No geofence support in unknown vehicle
_sendError
(
InternalError
,
QStringLiteral
(
"Geo-Fence write attempted while read in progress."
));
return
;
}
if
(
!
_geoFenceSupported
())
{
return
;
}
// Validate
switch
(
geoFence
.
fenceType
)
{
case
GeoFencePolygon
:
if
(
geoFence
.
polygon
.
count
()
<
3
)
{
_sendError
(
TooFewPoints
,
QStringLiteral
(
"Geo-Fence polygon must contain at least 3 points."
));
return
;
}
if
(
geoFence
.
polygon
.
count
()
>
std
::
numeric_limits
<
uint8_t
>::
max
())
{
_sendError
(
TooManyPoints
,
QStringLiteral
(
"Geo-Fence polygon has too many points: %1."
).
arg
(
geoFence
.
polygon
.
count
()));
return
;
}
break
;
case
GeoFenceCircle
:
if
(
geoFence
.
circleRadius
<=
0.0
)
{
_sendError
(
InvalidCircleRadius
,
QStringLiteral
(
"Geo-Fence circle radius must be greater than 0."
));
return
;
}
default:
break
;
}
_geoFence
.
fenceType
=
geoFence
.
fenceType
;
_geoFence
.
circleRadius
=
geoFence
.
circleRadius
;
_geoFence
.
polygon
=
geoFence
.
polygon
;
_geoFence
.
breachReturnPoint
=
geoFence
.
breachReturnPoint
;
if
(
_geoFence
.
fenceType
!=
GeoFencePolygon
)
{
// Circle is just params, so no more work to do
return
;
}
emit
newGeoFenceAvailable
();
// First thing is to turn off geo fence while we are updating. This prevents the vehicle from going haywire it is in the air
Fact
*
fenceActionFact
=
_vehicle
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
_fenceActionParam
);
_savedWriteFenceAction
=
fenceActionFact
->
rawValue
();
fenceActionFact
->
setRawValue
(
0
);
// Fence total param includes:
// index 0: breach return
// last index: polygon close (same as first polygon point)
_vehicle
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
_fenceTotalParam
)
->
setRawValue
(
_geoFence
.
polygon
.
count
()
+
2
);
// FIXME: No validation of correct fence received
// Send points:
// breach return
// polygon fence points
// polygon close
// Put back previous fence action to start geofence again
_sendFencePoint
(
0
);
for
(
uint8_t
index
=
0
;
index
<
_geoFence
.
polygon
.
count
();
index
++
)
{
_sendFencePoint
(
index
+
1
);
}
_sendFencePoint
(
_geoFence
.
polygon
.
count
()
+
1
);
fenceActionFact
->
setRawValue
(
_savedWriteFenceAction
);
}
}
void
GeoFenceManager
::
requestGeoFenc
e
(
void
)
void
GeoFenceManager
::
sendToVehicl
e
(
void
)
{
{
_clearGeoFence
();
// No geofence support in unknown vehicle
if
(
!
_geoFenceSupported
())
{
return
;
}
// Point 0: Breach return point
// Point [1,N]: Polygon points
// Point N+1: Close polygon point (same as point 1)
int
cFencePoints
=
_vehicle
->
getParameterFact
(
FactSystem
::
defaultComponentId
,
_fenceTotalParam
)
->
rawValue
().
toInt
();
qCDebug
(
GeoFenceManagerLog
)
<<
"GeoFenceManager::requestGeoFence"
<<
cFencePoints
;
if
(
cFencePoints
==
0
)
{
// No fence, no more work to do, fence data has already been cleared
return
;
}
if
(
cFencePoints
<
0
||
(
cFencePoints
>
0
&&
cFencePoints
<
6
))
{
_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 out vehicle is received
void
GeoFenceManager
::
_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
);
if
(
fencePoint
.
idx
!=
_currentFencePoint
)
{
// FIXME: Protocol out of whack
qCWarning
(
GeoFenceManagerLog
)
<<
"Indices out of sync"
<<
fencePoint
.
idx
<<
_currentFencePoint
;
return
;
}
if
(
fencePoint
.
idx
==
0
)
{
_geoFence
.
breachReturnPoint
=
QGeoCoordinate
(
fencePoint
.
lat
,
fencePoint
.
lng
);
qCDebug
(
GeoFenceManagerLog
)
<<
"From vehicle: breach return point"
<<
_geoFence
.
breachReturnPoint
;
_requestFencePoint
(
++
_currentFencePoint
);
}
else
if
(
fencePoint
.
idx
<
_cReadFencePoints
-
1
)
{
QGeoCoordinate
polyCoord
(
fencePoint
.
lat
,
fencePoint
.
lng
);
_geoFence
.
polygon
.
addCoordinate
(
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
_readTransactionInProgress
=
false
;
emit
newGeoFenceAvailable
();
}
}
}
}
void
GeoFenceManager
::
_clearGeoFence
(
void
)
{
_geoFence
.
fenceType
=
GeoFenceNone
;
_geoFence
.
circleRadius
=
0.0
;
_geoFence
.
polygon
.
clear
();
_geoFence
.
breachReturnPoint
=
QGeoCoordinate
();
emit
newGeoFenceAvailable
();
}
}
void
GeoFenceManager
::
_requestFencePoint
(
uint8_t
pointIndex
)
void
GeoFenceManager
::
setPolygon
(
QGCMapPolygon
*
polygon
)
{
{
mavlink_message_t
msg
;
_polygon
.
clear
();
MAVLinkProtocol
*
mavlink
=
qgcApp
()
->
toolbox
()
->
mavlinkProtocol
();
for
(
int
index
=
0
;
index
<
polygon
->
count
();
index
++
)
{
_polygon
.
append
((
*
polygon
)[
index
]);
qCDebug
(
GeoFenceManagerLog
)
<<
"GeoFenceManager::_requestFencePoint"
<<
pointIndex
;
mavlink_msg_fence_fetch_point_pack
(
mavlink
->
getSystemId
(),
mavlink
->
getComponentId
(),
&
msg
,
_vehicle
->
id
(),
_vehicle
->
defaultComponentId
(),
pointIndex
);
_vehicle
->
sendMessageOnPriorityLink
(
msg
);
}
void
GeoFenceManager
::
_sendFencePoint
(
uint8_t
pointIndex
)
{
mavlink_message_t
msg
;
MAVLinkProtocol
*
mavlink
=
qgcApp
()
->
toolbox
()
->
mavlinkProtocol
();
QGeoCoordinate
fenceCoord
;
if
(
pointIndex
==
0
)
{
fenceCoord
=
_geoFence
.
breachReturnPoint
;
}
else
if
(
pointIndex
-
1
<
_geoFence
.
polygon
.
count
())
{
fenceCoord
=
_geoFence
.
polygon
[
pointIndex
-
1
];
}
else
{
// Polygon close point
fenceCoord
=
_geoFence
.
polygon
[
0
];
}
}
emit
polygonChanged
(
_polygon
);
mavlink_msg_fence_point_pack
(
mavlink
->
getSystemId
(),
mavlink
->
getComponentId
(),
&
msg
,
_vehicle
->
id
(),
_vehicle
->
defaultComponentId
(),
pointIndex
,
// Index of point to set
_geoFence
.
polygon
.
count
()
+
2
,
// Total point count, +1 for breach in index 0, +1 polygon close in last index
fenceCoord
.
latitude
(),
fenceCoord
.
longitude
());
_vehicle
->
sendMessageOnPriorityLink
(
msg
);
}
}
bool
GeoFenceManager
::
inProgress
(
void
)
const
void
GeoFenceManager
::
setBreachReturnPoint
(
const
QGeoCoordinate
&
breachReturnPoint
)
{
{
return
_readTransactionInProgress
||
_writeTransactionInProgress
;
if
(
breachReturnPoint
!=
_breachReturnPoint
)
{
}
_breachReturnPoint
=
breachReturnPoint
;
emit
breachReturnPointChanged
(
breachReturnPoint
);
bool
GeoFenceManager
::
_geoFenceSupported
(
void
)
{
// FIXME: MockLink doesn't support geo fence yet
if
(
qgcApp
()
->
runningUnitTests
())
{
return
false
;
}
// FIXME: Hack to get around lack of plugin-ized version of code
if
(
_vehicle
->
apmFirmware
())
{
if
(
!
_vehicle
->
parameterExists
(
FactSystem
::
defaultComponentId
,
_fenceTotalParam
)
||
!
_vehicle
->
parameterExists
(
FactSystem
::
defaultComponentId
,
_fenceActionParam
))
{
_sendError
(
InternalError
,
QStringLiteral
(
"Vehicle does not support Geo-Fence implementation."
));
return
false
;
}
else
{
return
true
;
}
}
else
{
return
false
;
}
}
}
}
src/MissionManager/GeoFenceManager.h
View file @
d9fa1118
...
@@ -11,19 +11,17 @@
...
@@ -11,19 +11,17 @@
#define GeoFenceManager_H
#define GeoFenceManager_H
#include <QObject>
#include <QObject>
#include <QTimer>
#include <QGeoCoordinate>
#include <QGeoCoordinate>
#include "MissionItem.h"
#include "QGCMAVLink.h"
#include "QGCLoggingCategory.h"
#include "QGCLoggingCategory.h"
#include "LinkInterface.h"
#include "QGCMapPolygon.h"
#include "QGCMapPolygon.h"
class
Vehicle
;
class
Vehicle
;
Q_DECLARE_LOGGING_CATEGORY
(
GeoFenceManagerLog
)
Q_DECLARE_LOGGING_CATEGORY
(
GeoFenceManagerLog
)
/// This is the base class for firmware specific geofence managers. A geofence manager is responsible
/// for communicating with the vehicle to set/get geofence settings.
class
GeoFenceManager
:
public
QObject
class
GeoFenceManager
:
public
QObject
{
{
Q_OBJECT
Q_OBJECT
...
@@ -32,29 +30,30 @@ public:
...
@@ -32,29 +30,30 @@ public:
GeoFenceManager
(
Vehicle
*
vehicle
);
GeoFenceManager
(
Vehicle
*
vehicle
);
~
GeoFenceManager
();
~
GeoFenceManager
();
typedef
enum
{
/// Returns true if the manager is currently communicating with the vehicle
GeoFenceNone
,
virtual
bool
inProgress
(
void
)
const
{
return
false
;
}
GeoFenceCircle
,
GeoFencePolygon
,
/// Load the current settings from teh vehicle
}
GeoFenceType_t
;
virtual
void
loadFromVehicle
(
void
)
;
typedef
struct
{
/// Send the current settings to the vehicle
GeoFenceType_t
fenceType
;
virtual
void
sendToVehicle
(
void
);
float
circleRadius
;
QGCMapPolygon
polygon
;
QGeoCoordinate
breachReturnPoint
;
}
GeoFence_t
;
bool
inProgress
(
void
)
const
;
// Support flags
virtual
bool
fenceSupported
(
void
)
const
{
return
false
;
}
virtual
bool
circleSupported
(
void
)
const
{
return
false
;
}
virtual
bool
polygonSupported
(
void
)
const
{
return
false
;
}
virtual
bool
breachReturnSupported
(
void
)
const
{
return
false
;
}
/// Request the geo fence from the vehicle
virtual
float
circleRadius
(
void
)
const
{
return
0
.
0
;
}
void
requestGeoFence
(
void
);
QList
<
QGeoCoordinate
>
polygon
(
void
)
const
{
return
_polygon
;
}
QGeoCoordinate
breachReturnPoint
(
void
)
const
{
return
_breachReturnPoint
;
}
/// Returns the current geofence settings
virtual
QVariantList
params
(
void
)
const
{
return
QVariantList
();
}
const
GeoFence_t
&
geoFence
(
void
)
const
{
return
_geoFence
;
}
virtual
QStringList
paramLabels
(
void
)
const
{
return
QStringList
()
;
}
/// Set and send the specified geo fence to the vehicle
void
setPolygon
(
QGCMapPolygon
*
polygon
);
void
set
GeoFence
(
const
GeoFence_t
&
geoFence
);
void
set
BreachReturnPoint
(
const
QGeoCoordinate
&
breachReturnPoint
);
/// Error codes returned in error signal
/// Error codes returned in error signal
typedef
enum
{
typedef
enum
{
...
@@ -65,35 +64,24 @@ public:
...
@@ -65,35 +64,24 @@ public:
}
ErrorCode_t
;
}
ErrorCode_t
;
signals:
signals:
void
newGeoFenceAvailable
(
void
);
void
fenceSupportedChanged
(
bool
fenceSupported
);
void
inProgressChanged
(
bool
inProgress
);
void
circleSupportedChanged
(
bool
circleSupported
);
void
error
(
int
errorCode
,
const
QString
&
errorMsg
);
void
polygonSupportedChanged
(
bool
polygonSupported
);
void
breachReturnSupportedChanged
(
bool
fenceSupported
);
void
circleRadiusChanged
(
float
circleRadius
);
void
polygonChanged
(
QList
<
QGeoCoordinate
>
polygon
);
void
breachReturnPointChanged
(
QGeoCoordinate
breachReturnPoint
);
void
inProgressChanged
(
bool
inProgress
);
void
error
(
int
errorCode
,
const
QString
&
errorMsg
);
void
paramsChanged
(
QVariantList
params
);
void
paramLabelsChanged
(
QStringList
paramLabels
);
private
slots
:
protected:
void
_mavlinkMessageReceived
(
const
mavlink_message_t
&
message
);
//void _ackTimeout(void);
private:
void
_sendError
(
ErrorCode_t
errorCode
,
const
QString
&
errorMsg
);
void
_sendError
(
ErrorCode_t
errorCode
,
const
QString
&
errorMsg
);
void
_clearGeoFence
(
void
);
void
_requestFencePoint
(
uint8_t
pointIndex
);
void
_sendFencePoint
(
uint8_t
pointIndex
);
bool
_geoFenceSupported
(
void
);
private:
Vehicle
*
_vehicle
;
bool
_readTransactionInProgress
;
bool
_writeTransactionInProgress
;
uint8_t
_cReadFencePoints
;
uint8_t
_currentFencePoint
;
QVariant
_savedWriteFenceAction
;
GeoFence_t
_geoFence
;
static
const
char
*
_fenceTotalParam
;
Vehicle
*
_vehicle
;
static
const
char
*
_fenceActionParam
;
QList
<
QGeoCoordinate
>
_polygon
;
QGeoCoordinate
_breachReturnPoint
;
};
};
#endif
#endif
src/MissionManager/MissionController.cc
View file @
d9fa1118
...
@@ -623,8 +623,6 @@ void MissionController::_recalcWaypointLines(void)
...
@@ -623,8 +623,6 @@ void MissionController::_recalcWaypointLines(void)
if
(
!
homeItem
)
{
if
(
!
homeItem
)
{
qWarning
()
<<
"Home item is not SimpleMissionItem"
;
qWarning
()
<<
"Home item is not SimpleMissionItem"
;
}
else
{
connect
(
homeItem
,
&
VisualMissionItem
::
coordinateChanged
,
this
,
&
MissionController
::
_recalcAltitudeRangeBearing
);
}
}
bool
showHomePosition
=
homeItem
->
showHomePosition
();
bool
showHomePosition
=
homeItem
->
showHomePosition
();
...
@@ -944,6 +942,10 @@ void MissionController::_initAllVisualItems(void)
...
@@ -944,6 +942,10 @@ void MissionController::_initAllVisualItems(void)
homeItem
->
setShowHomePosition
(
true
);
homeItem
->
setShowHomePosition
(
true
);
}
}
emit
plannedHomePositionChanged
(
plannedHomePosition
());
connect
(
homeItem
,
&
VisualMissionItem
::
coordinateChanged
,
this
,
&
MissionController
::
_homeCoordinateChanged
);
QmlObjectListModel
*
newComplexItems
=
new
QmlObjectListModel
(
this
);
QmlObjectListModel
*
newComplexItems
=
new
QmlObjectListModel
(
this
);
for
(
int
i
=
0
;
i
<
_visualItems
->
count
();
i
++
)
{
for
(
int
i
=
0
;
i
<
_visualItems
->
count
();
i
++
)
{
VisualMissionItem
*
item
=
qobject_cast
<
VisualMissionItem
*>
(
_visualItems
->
get
(
i
));
VisualMissionItem
*
item
=
qobject_cast
<
VisualMissionItem
*>
(
_visualItems
->
get
(
i
));
...
@@ -1021,17 +1023,17 @@ void MissionController::_itemCommandChanged(void)
...
@@ -1021,17 +1023,17 @@ void MissionController::_itemCommandChanged(void)
_recalcWaypointLines
();
_recalcWaypointLines
();
}
}
void
MissionController
::
_activeVehicleBeingRemoved
(
void
)
void
MissionController
::
_activeVehicleBeingRemoved
(
Vehicle
*
vehicle
)
{
{
qCDebug
(
MissionControllerLog
)
<<
"_activeVehicleSet _activeVehicleBeingRemoved"
;
qCDebug
(
MissionControllerLog
)
<<
"_activeVehicleSet _activeVehicleBeingRemoved"
;
MissionManager
*
missionManager
=
_activeV
ehicle
->
missionManager
();
MissionManager
*
missionManager
=
v
ehicle
->
missionManager
();
disconnect
(
missionManager
,
&
MissionManager
::
newMissionItemsAvailable
,
this
,
&
MissionController
::
_newMissionItemsAvailableFromVehicle
);
disconnect
(
missionManager
,
&
MissionManager
::
newMissionItemsAvailable
,
this
,
&
MissionController
::
_newMissionItemsAvailableFromVehicle
);
disconnect
(
missionManager
,
&
MissionManager
::
inProgressChanged
,
this
,
&
MissionController
::
_inProgressChanged
);
disconnect
(
missionManager
,
&
MissionManager
::
inProgressChanged
,
this
,
&
MissionController
::
_inProgressChanged
);
disconnect
(
missionManager
,
&
MissionManager
::
currentItemChanged
,
this
,
&
MissionController
::
_currentMissionItemChanged
);
disconnect
(
missionManager
,
&
MissionManager
::
currentItemChanged
,
this
,
&
MissionController
::
_currentMissionItemChanged
);
disconnect
(
_activeVehicle
,
&
Vehicle
::
homePositionAvailableChanged
,
this
,
&
MissionController
::
_activeVehicleHomePositionAvailableChanged
);
disconnect
(
vehicle
,
&
Vehicle
::
homePositionAvailableChanged
,
this
,
&
MissionController
::
_activeVehicleHomePositionAvailableChanged
);
disconnect
(
_activeVehicle
,
&
Vehicle
::
homePositionChanged
,
this
,
&
MissionController
::
_activeVehicleHomePositionChanged
);
disconnect
(
vehicle
,
&
Vehicle
::
homePositionChanged
,
this
,
&
MissionController
::
_activeVehicleHomePositionChanged
);
// We always remove all items on vehicle change. This leaves a user model hole:
// We always remove all items on vehicle change. This leaves a user model hole:
// If the user has unsaved changes in the Plan view they will lose them
// If the user has unsaved changes in the Plan view they will lose them
...
@@ -1069,6 +1071,7 @@ void MissionController::_activeVehicleHomePositionAvailableChanged(bool homePosi
...
@@ -1069,6 +1071,7 @@ void MissionController::_activeVehicleHomePositionAvailableChanged(bool homePosi
if
(
homeItem
)
{
if
(
homeItem
)
{
homeItem
->
setShowHomePosition
(
homePositionAvailable
);
homeItem
->
setShowHomePosition
(
homePositionAvailable
);
emit
plannedHomePositionChanged
(
plannedHomePosition
());
_recalcWaypointLines
();
_recalcWaypointLines
();
}
else
{
}
else
{
qWarning
()
<<
"Unabled to cast home item to SimpleMissionItem"
;
qWarning
()
<<
"Unabled to cast home item to SimpleMissionItem"
;
...
@@ -1079,8 +1082,17 @@ void MissionController::_activeVehicleHomePositionAvailableChanged(bool homePosi
...
@@ -1079,8 +1082,17 @@ void MissionController::_activeVehicleHomePositionAvailableChanged(bool homePosi
void
MissionController
::
_activeVehicleHomePositionChanged
(
const
QGeoCoordinate
&
homePosition
)
void
MissionController
::
_activeVehicleHomePositionChanged
(
const
QGeoCoordinate
&
homePosition
)
{
{
if
(
!
_editMode
&&
_visualItems
)
{
if
(
!
_editMode
&&
_visualItems
)
{
qobject_cast
<
VisualMissionItem
*>
(
_visualItems
->
get
(
0
))
->
setCoordinate
(
homePosition
);
VisualMissionItem
*
item
=
qobject_cast
<
VisualMissionItem
*>
(
_visualItems
->
get
(
0
));
_recalcWaypointLines
();
if
(
item
)
{
if
(
item
->
coordinate
()
!=
homePosition
)
{
item
->
setCoordinate
(
homePosition
);
qCDebug
(
MissionControllerLog
)
<<
"Home position update"
<<
homePosition
;
emit
plannedHomePositionChanged
(
plannedHomePosition
());
_recalcWaypointLines
();
}
}
else
{
qWarning
()
<<
"Unabled to cast home item to VisualMissionItem"
;
}
}
}
}
}
...
@@ -1256,3 +1268,21 @@ void MissionController::setDirty(bool dirty)
...
@@ -1256,3 +1268,21 @@ void MissionController::setDirty(bool dirty)
_visualItems
->
setDirty
(
dirty
);
_visualItems
->
setDirty
(
dirty
);
}
}
}
}
QGeoCoordinate
MissionController
::
plannedHomePosition
(
void
)
{
if
(
_visualItems
&&
_visualItems
->
count
()
>
0
)
{
SimpleMissionItem
*
item
=
qobject_cast
<
SimpleMissionItem
*>
(
_visualItems
->
get
(
0
));
if
(
item
&&
item
->
showHomePosition
())
{
return
item
->
coordinate
();
}
}
return
QGeoCoordinate
();
}
void
MissionController
::
_homeCoordinateChanged
(
void
)
{
emit
plannedHomePositionChanged
(
plannedHomePosition
());
_recalcAltitudeRangeBearing
();
}
src/MissionManager/MissionController.h
View file @
d9fa1118
...
@@ -34,6 +34,7 @@ public:
...
@@ -34,6 +34,7 @@ public:
MissionController
(
QObject
*
parent
=
NULL
);
MissionController
(
QObject
*
parent
=
NULL
);
~
MissionController
();
~
MissionController
();
Q_PROPERTY
(
QGeoCoordinate
plannedHomePosition
READ
plannedHomePosition
NOTIFY
plannedHomePositionChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
visualItems
READ
visualItems
NOTIFY
visualItemsChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
visualItems
READ
visualItems
NOTIFY
visualItemsChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
complexVisualItems
READ
complexVisualItems
NOTIFY
complexVisualItemsChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
complexVisualItems
READ
complexVisualItems
NOTIFY
complexVisualItemsChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
waypointLines
READ
waypointLines
NOTIFY
waypointLinesChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
waypointLines
READ
waypointLines
NOTIFY
waypointLinesChanged
)
...
@@ -70,6 +71,7 @@ public:
...
@@ -70,6 +71,7 @@ public:
// Property accessors
// Property accessors
QGeoCoordinate
plannedHomePosition
(
void
);
QmlObjectListModel
*
visualItems
(
void
)
{
return
_visualItems
;
}
QmlObjectListModel
*
visualItems
(
void
)
{
return
_visualItems
;
}
QmlObjectListModel
*
complexVisualItems
(
void
)
{
return
_complexItems
;
}
QmlObjectListModel
*
complexVisualItems
(
void
)
{
return
_complexItems
;
}
QmlObjectListModel
*
waypointLines
(
void
)
{
return
&
_waypointLines
;
}
QmlObjectListModel
*
waypointLines
(
void
)
{
return
&
_waypointLines
;
}
...
@@ -79,7 +81,6 @@ public:
...
@@ -79,7 +81,6 @@ public:
double
cruiseDistance
(
void
)
const
{
return
_cruiseDistance
;
}
double
cruiseDistance
(
void
)
const
{
return
_cruiseDistance
;
}
double
hoverDistance
(
void
)
const
{
return
_hoverDistance
;
}
double
hoverDistance
(
void
)
const
{
return
_hoverDistance
;
}
void
setMissionDistance
(
double
missionDistance
);
void
setMissionDistance
(
double
missionDistance
);
void
setMissionMaxTelemetry
(
double
missionMaxTelemetry
);
void
setMissionMaxTelemetry
(
double
missionMaxTelemetry
);
void
setCruiseDistance
(
double
cruiseDistance
);
void
setCruiseDistance
(
double
cruiseDistance
);
...
@@ -88,6 +89,7 @@ public:
...
@@ -88,6 +89,7 @@ public:
static
const
char
*
jsonSimpleItemsKey
;
///< Key for simple items in a json file
static
const
char
*
jsonSimpleItemsKey
;
///< Key for simple items in a json file
signals:
signals:
void
plannedHomePositionChanged
(
QGeoCoordinate
plannedHomePosition
);
void
visualItemsChanged
(
void
);
void
visualItemsChanged
(
void
);
void
complexVisualItemsChanged
(
void
);
void
complexVisualItemsChanged
(
void
);
void
waypointLinesChanged
(
void
);
void
waypointLinesChanged
(
void
);
...
@@ -105,7 +107,8 @@ private slots:
...
@@ -105,7 +107,8 @@ private slots:
void
_inProgressChanged
(
bool
inProgress
);
void
_inProgressChanged
(
bool
inProgress
);
void
_currentMissionItemChanged
(
int
sequenceNumber
);
void
_currentMissionItemChanged
(
int
sequenceNumber
);
void
_recalcWaypointLines
(
void
);
void
_recalcWaypointLines
(
void
);
void
_recalcAltitudeRangeBearing
();
void
_recalcAltitudeRangeBearing
(
void
);
void
_homeCoordinateChanged
(
void
);
private:
private:
void
_recalcSequence
(
void
);
void
_recalcSequence
(
void
);
...
@@ -128,7 +131,7 @@ private:
...
@@ -128,7 +131,7 @@ private:
int
_nextSequenceNumber
(
void
);
int
_nextSequenceNumber
(
void
);
// Overrides from PlanElementController
// Overrides from PlanElementController
void
_activeVehicleBeingRemoved
(
void
)
final
;
void
_activeVehicleBeingRemoved
(
Vehicle
*
vehicle
)
final
;
void
_activeVehicleSet
(
void
)
final
;
void
_activeVehicleSet
(
void
)
final
;
private:
private:
...
...
src/MissionManager/MissionManager.cc
View file @
d9fa1118
...
@@ -574,8 +574,8 @@ void MissionManager::_handleMissionCurrent(const mavlink_message_t& message)
...
@@ -574,8 +574,8 @@ void MissionManager::_handleMissionCurrent(const mavlink_message_t& message)
mavlink_msg_mission_current_decode
(
&
message
,
&
missionCurrent
);
mavlink_msg_mission_current_decode
(
&
message
,
&
missionCurrent
);
qCDebug
(
MissionManagerLog
)
<<
"_handleMissionCurrent seq:"
<<
missionCurrent
.
seq
;
if
(
missionCurrent
.
seq
!=
_currentMissionItem
)
{
if
(
missionCurrent
.
seq
!=
_currentMissionItem
)
{
qCDebug
(
MissionManagerLog
)
<<
"_handleMissionCurrent seq:"
<<
missionCurrent
.
seq
;
_currentMissionItem
=
missionCurrent
.
seq
;
_currentMissionItem
=
missionCurrent
.
seq
;
emit
currentItemChanged
(
_currentMissionItem
);
emit
currentItemChanged
(
_currentMissionItem
);
}
}
...
...
src/MissionManager/PlanElementController.cc
View file @
d9fa1118
...
@@ -34,7 +34,9 @@ void PlanElementController::start(bool editMode)
...
@@ -34,7 +34,9 @@ void PlanElementController::start(bool editMode)
void
PlanElementController
::
_activeVehicleChanged
(
Vehicle
*
activeVehicle
)
void
PlanElementController
::
_activeVehicleChanged
(
Vehicle
*
activeVehicle
)
{
{
if
(
_activeVehicle
)
{
if
(
_activeVehicle
)
{
_activeVehicleBeingRemoved
();
Vehicle
*
vehicleSave
=
_activeVehicle
;
_activeVehicle
=
NULL
;
_activeVehicleBeingRemoved
(
vehicleSave
);
}
}
_activeVehicle
=
activeVehicle
;
_activeVehicle
=
activeVehicle
;
...
...
src/MissionManager/PlanElementController.h
View file @
d9fa1118
...
@@ -57,8 +57,8 @@ protected:
...
@@ -57,8 +57,8 @@ protected:
bool
_editMode
;
bool
_editMode
;
/// Called when the current active vehicle has been removed. Derived classes should override
/// Called when the current active vehicle has been removed. Derived classes should override
/// to implement custom behavior.
/// to implement custom behavior.
When this is called _activeVehicle has already been cleared.
virtual
void
_activeVehicleBeingRemoved
(
void
)
=
0
;
virtual
void
_activeVehicleBeingRemoved
(
Vehicle
*
removedVehicle
)
=
0
;
/// Called when a new active vehicle has been set. Derived classes should override
/// Called when a new active vehicle has been set. Derived classes should override
/// to implement custom behavior.
/// to implement custom behavior.
...
...
src/Vehicle/Vehicle.cc
View file @
d9fa1118
...
@@ -199,13 +199,14 @@ Vehicle::Vehicle(LinkInterface* link,
...
@@ -199,13 +199,14 @@ Vehicle::Vehicle(LinkInterface* link,
connect
(
_missionManager
,
&
MissionManager
::
error
,
this
,
&
Vehicle
::
_missionManagerError
);
connect
(
_missionManager
,
&
MissionManager
::
error
,
this
,
&
Vehicle
::
_missionManagerError
);
connect
(
_missionManager
,
&
MissionManager
::
newMissionItemsAvailable
,
this
,
&
Vehicle
::
_newMissionItemsAvailable
);
connect
(
_missionManager
,
&
MissionManager
::
newMissionItemsAvailable
,
this
,
&
Vehicle
::
_newMissionItemsAvailable
);
_geoFenceManager
=
new
GeoFenceManager
(
this
);
connect
(
_geoFenceManager
,
&
GeoFenceManager
::
error
,
this
,
&
Vehicle
::
_geoFenceManagerError
);
_parameterLoader
=
new
ParameterLoader
(
this
);
_parameterLoader
=
new
ParameterLoader
(
this
);
connect
(
_parameterLoader
,
&
ParameterLoader
::
parametersReady
,
_autopilotPlugin
,
&
AutoPilotPlugin
::
_parametersReadyPreChecks
);
connect
(
_parameterLoader
,
&
ParameterLoader
::
parametersReady
,
_autopilotPlugin
,
&
AutoPilotPlugin
::
_parametersReadyPreChecks
);
connect
(
_parameterLoader
,
&
ParameterLoader
::
parameterListProgress
,
_autopilotPlugin
,
&
AutoPilotPlugin
::
parameterListProgress
);
connect
(
_parameterLoader
,
&
ParameterLoader
::
parameterListProgress
,
_autopilotPlugin
,
&
AutoPilotPlugin
::
parameterListProgress
);
// GeoFenceManager needs to access ParameterLoader so make sure to create afters
_geoFenceManager
=
_firmwarePlugin
->
newGeoFenceManager
(
this
);
connect
(
_geoFenceManager
,
&
GeoFenceManager
::
error
,
this
,
&
Vehicle
::
_geoFenceManagerError
);
// Ask the vehicle for firmware version info. This must be MAV_COMP_ID_ALL since we don't know default component id yet.
// Ask the vehicle for firmware version info. This must be MAV_COMP_ID_ALL since we don't know default component id yet.
mavlink_message_t
versionMsg
;
mavlink_message_t
versionMsg
;
...
@@ -1826,7 +1827,7 @@ void Vehicle::motorTest(int motor, int percent, int timeoutSecs)
...
@@ -1826,7 +1827,7 @@ void Vehicle::motorTest(int motor, int percent, int timeoutSecs)
#endif
#endif
/// Returns true if the specifed parameter exists from the default component
/// Returns true if the specifed parameter exists from the default component
bool
Vehicle
::
parameterExists
(
int
componentId
,
const
QString
&
name
)
bool
Vehicle
::
parameterExists
(
int
componentId
,
const
QString
&
name
)
const
{
{
return
_autopilotPlugin
->
parameterExists
(
componentId
,
name
);
return
_autopilotPlugin
->
parameterExists
(
componentId
,
name
);
}
}
...
@@ -1844,7 +1845,7 @@ void Vehicle::_newMissionItemsAvailable(void)
...
@@ -1844,7 +1845,7 @@ void Vehicle::_newMissionItemsAvailable(void)
// After the initial mission request complets we ask for the geofence
// After the initial mission request complets we ask for the geofence
if
(
!
_geoFenceManagerInitialRequestComplete
)
{
if
(
!
_geoFenceManagerInitialRequestComplete
)
{
_geoFenceManagerInitialRequestComplete
=
true
;
_geoFenceManagerInitialRequestComplete
=
true
;
_geoFenceManager
->
requestGeoFenc
e
();
_geoFenceManager
->
loadFromVehicl
e
();
}
}
}
}
...
...
src/Vehicle/Vehicle.h
View file @
d9fa1118
...
@@ -570,7 +570,7 @@ public:
...
@@ -570,7 +570,7 @@ public:
bool
xConfigMotors
(
void
);
bool
xConfigMotors
(
void
);
/// Returns true if the specifed parameter exists from the default component
/// Returns true if the specifed parameter exists from the default component
bool
parameterExists
(
int
componentId
,
const
QString
&
name
);
bool
parameterExists
(
int
componentId
,
const
QString
&
name
)
const
;
/// Returns the specified parameter Fact from the default component
/// Returns the specified parameter Fact from the default component
/// WARNING: Returns a default Fact if parameter does not exists. If that possibility exists, check for existence first with
/// WARNING: Returns a default Fact if parameter does not exists. If that possibility exists, check for existence first with
...
...
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