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
790d03b4
Commit
790d03b4
authored
Jan 30, 2018
by
DonLakeFlyer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Initial Corridor Scan support
parent
8bf441af
Changes
32
Expand all
Hide whitespace changes
Inline
Side-by-side
Showing
32 changed files
with
2393 additions
and
16 deletions
+2393
-16
UnitTest.qrc
UnitTest.qrc
+5
-4
qgroundcontrol.pro
qgroundcontrol.pro
+8
-0
qgroundcontrol.qrc
qgroundcontrol.qrc
+4
-0
CameraCalc.h
src/MissionManager/CameraCalc.h
+8
-0
ComplexMissionItem.h
src/MissionManager/ComplexMissionItem.h
+2
-2
CorridorScan.SettingsGroup.json
src/MissionManager/CorridorScan.SettingsGroup.json
+46
-0
CorridorScanComplexItem.cc
src/MissionManager/CorridorScanComplexItem.cc
+417
-0
CorridorScanComplexItem.h
src/MissionManager/CorridorScanComplexItem.h
+138
-0
CorridorScanComplexItemTest.cc
src/MissionManager/CorridorScanComplexItemTest.cc
+217
-0
CorridorScanComplexItemTest.h
src/MissionManager/CorridorScanComplexItemTest.h
+84
-0
MissionController.cc
src/MissionManager/MissionController.cc
+14
-1
MissionController.h
src/MissionManager/MissionController.h
+1
-0
QGCMapPolygon.cc
src/MissionManager/QGCMapPolygon.cc
+17
-1
QGCMapPolygon.h
src/MissionManager/QGCMapPolygon.h
+4
-1
QGCMapPolygonTest.cc
src/MissionManager/QGCMapPolygonTest.cc
+10
-4
QGCMapPolygonTest.h
src/MissionManager/QGCMapPolygonTest.h
+1
-0
QGCMapPolyline.cc
src/MissionManager/QGCMapPolyline.cc
+440
-0
QGCMapPolyline.h
src/MissionManager/QGCMapPolyline.h
+114
-0
QGCMapPolylineTest.cc
src/MissionManager/QGCMapPolylineTest.cc
+202
-0
QGCMapPolylineTest.h
src/MissionManager/QGCMapPolylineTest.h
+71
-0
QGCMapPolylineVisuals.qml
src/MissionManager/QGCMapPolylineVisuals.qml
+287
-0
StructureScanComplexItem.cc
src/MissionManager/StructureScanComplexItem.cc
+1
-1
SurveyMissionItem.cc
src/MissionManager/SurveyMissionItem.cc
+2
-2
PolygonAreaTest.kml
src/MissionManager/UnitTest/PolygonAreaTest.kml
+48
-0
PolygonBadCoordinatesNode.kml
src/MissionManager/UnitTest/PolygonBadCoordinatesNode.kml
+0
-0
PolygonBadXml.kml
src/MissionManager/UnitTest/PolygonBadXml.kml
+0
-0
PolygonGood.kml
src/MissionManager/UnitTest/PolygonGood.kml
+0
-0
PolygonMissingNode.kml
src/MissionManager/UnitTest/PolygonMissingNode.kml
+0
-0
CorridorScanEditor.qml
src/PlanView/CorridorScanEditor.qml
+122
-0
CorridorScanMapVisual.qml
src/PlanView/CorridorScanMapVisual.qml
+125
-0
QGroundControl.Controls.qmldir
src/QmlControls/QGroundControl.Controls.qmldir
+1
-0
UnitTestList.cc
src/qgcunittest/UnitTestList.cc
+4
-0
No files found.
UnitTest.qrc
View file @
790d03b4
...
...
@@ -9,9 +9,10 @@
<file alias="MavCmdInfoVTOL.json">src/MissionManager/UnitTest/MavCmdInfoVTOL.json</file>
<file alias="MissionPlanner.waypoints">src/MissionManager/UnitTest/MissionPlanner.waypoints</file>
<file alias="OldFileFormat.mission">src/MissionManager/UnitTest/OldFileFormat.mission</file>
<file alias="GoodPolygon.kml">src/MissionManager/UnitTest/GoodPolygon.kml</file>
<file alias="MissingPolygonNode.kml">src/MissionManager/UnitTest/MissingPolygonNode.kml</file>
<file alias="BadXml.kml">src/MissionManager/UnitTest/BadXml.kml</file>
<file alias="BadCoordinatesNode.kml">src/MissionManager/UnitTest/BadCoordinatesNode.kml</file>
<file alias="PolygonAreaTest.kml">src/MissionManager/UnitTest/PolygonAreaTest.kml</file>
<file alias="PolygonGood.kml">src/MissionManager/UnitTest/PolygonGood.kml</file>
<file alias="PolygonMissingNode.kml">src/MissionManager/UnitTest/PolygonMissingNode.kml</file>
<file alias="PolygonBadXml.kml">src/MissionManager/UnitTest/PolygonBadXml.kml</file>
<file alias="PolygonBadCoordinatesNode.kml">src/MissionManager/UnitTest/PolygonBadCoordinatesNode.kml</file>
</qresource>
</RCC>
qgroundcontrol.pro
View file @
790d03b4
...
...
@@ -415,6 +415,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin {
src
/
FactSystem
/
FactSystemTestPX4
.
h
\
src
/
FactSystem
/
ParameterManagerTest
.
h
\
src
/
MissionManager
/
CameraSectionTest
.
h
\
src
/
MissionManager
/
CorridorScanComplexItemTest
.
h
\
src
/
MissionManager
/
MissionCommandTreeTest
.
h
\
src
/
MissionManager
/
MissionControllerManagerTest
.
h
\
src
/
MissionManager
/
MissionControllerTest
.
h
\
...
...
@@ -423,6 +424,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin {
src
/
MissionManager
/
MissionSettingsTest
.
h
\
src
/
MissionManager
/
PlanMasterControllerTest
.
h
\
src
/
MissionManager
/
QGCMapPolygonTest
.
h
\
src
/
MissionManager
/
QGCMapPolylineTest
.
h
\
src
/
MissionManager
/
SectionTest
.
h
\
src
/
MissionManager
/
SimpleMissionItemTest
.
h
\
src
/
MissionManager
/
SpeedSectionTest
.
h
\
...
...
@@ -452,6 +454,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin {
src
/
FactSystem
/
FactSystemTestPX4
.
cc
\
src
/
FactSystem
/
ParameterManagerTest
.
cc
\
src
/
MissionManager
/
CameraSectionTest
.
cc
\
src
/
MissionManager
/
CorridorScanComplexItemTest
.
cc
\
src
/
MissionManager
/
MissionCommandTreeTest
.
cc
\
src
/
MissionManager
/
MissionControllerManagerTest
.
cc
\
src
/
MissionManager
/
MissionControllerTest
.
cc
\
...
...
@@ -460,6 +463,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin {
src
/
MissionManager
/
MissionSettingsTest
.
cc
\
src
/
MissionManager
/
PlanMasterControllerTest
.
cc
\
src
/
MissionManager
/
QGCMapPolygonTest
.
cc
\
src
/
MissionManager
/
QGCMapPolylineTest
.
cc
\
src
/
MissionManager
/
SectionTest
.
cc
\
src
/
MissionManager
/
SimpleMissionItemTest
.
cc
\
src
/
MissionManager
/
SpeedSectionTest
.
cc
\
...
...
@@ -508,6 +512,7 @@ HEADERS += \
src
/
MissionManager
/
CameraSection
.
h
\
src
/
MissionManager
/
CameraSpec
.
h
\
src
/
MissionManager
/
ComplexMissionItem
.
h
\
src
/
MissionManager
/
CorridorScanComplexItem
.
h
\
src
/
MissionManager
/
FixedWingLandingComplexItem
.
h
\
src
/
MissionManager
/
GeoFenceController
.
h
\
src
/
MissionManager
/
GeoFenceManager
.
h
\
...
...
@@ -526,6 +531,7 @@ HEADERS += \
src
/
MissionManager
/
QGCFencePolygon
.
h
\
src
/
MissionManager
/
QGCMapCircle
.
h
\
src
/
MissionManager
/
QGCMapPolygon
.
h
\
src
/
MissionManager
/
QGCMapPolyline
.
h
\
src
/
MissionManager
/
RallyPoint
.
h
\
src
/
MissionManager
/
RallyPointController
.
h
\
src
/
MissionManager
/
RallyPointManager
.
h
\
...
...
@@ -700,6 +706,7 @@ SOURCES += \
src
/
MissionManager
/
CameraSection
.
cc
\
src
/
MissionManager
/
CameraSpec
.
cc
\
src
/
MissionManager
/
ComplexMissionItem
.
cc
\
src
/
MissionManager
/
CorridorScanComplexItem
.
cc
\
src
/
MissionManager
/
FixedWingLandingComplexItem
.
cc
\
src
/
MissionManager
/
GeoFenceController
.
cc
\
src
/
MissionManager
/
GeoFenceManager
.
cc
\
...
...
@@ -718,6 +725,7 @@ SOURCES += \
src
/
MissionManager
/
QGCFencePolygon
.
cc
\
src
/
MissionManager
/
QGCMapCircle
.
cc
\
src
/
MissionManager
/
QGCMapPolygon
.
cc
\
src
/
MissionManager
/
QGCMapPolyline
.
cc
\
src
/
MissionManager
/
RallyPoint
.
cc
\
src
/
MissionManager
/
RallyPointController
.
cc
\
src
/
MissionManager
/
RallyPointManager
.
cc
\
...
...
qgroundcontrol.qrc
View file @
790d03b4
...
...
@@ -18,6 +18,7 @@
<file alias="AnalyzeView.qml">src/AnalyzeView/AnalyzeView.qml</file>
<file alias="AppSettings.qml">src/ui/AppSettings.qml</file>
<file alias="BluetoothSettings.qml">src/ui/preferences/BluetoothSettings.qml</file>
<file alias="CorridorScanEditor.qml">src/PlanView/CorridorScanEditor.qml</file>
<file alias="CustomCommandWidget.qml">src/ViewWidgets/CustomCommandWidget.qml</file>
<file alias="DebugWindow.qml">src/ui/preferences/DebugWindow.qml</file>
<file alias="ESP8266Component.qml">src/AutoPilotPlugins/Common/ESP8266Component.qml</file>
...
...
@@ -48,6 +49,7 @@
<file alias="QGroundControl/Controls/CameraCalc.qml">src/PlanView/CameraCalc.qml</file>
<file alias="QGroundControl/Controls/CameraSection.qml">src/PlanView/CameraSection.qml</file>
<file alias="QGroundControl/Controls/ClickableColor.qml">src/QmlControls/ClickableColor.qml</file>
<file alias="QGroundControl/Controls/CorridorScanMapVisual.qml">src/PlanView/CorridorScanMapVisual.qml</file>
<file alias="QGroundControl/Controls/DropButton.qml">src/QmlControls/DropButton.qml</file>
<file alias="QGroundControl/Controls/EditPositionDialog.qml">src/QmlControls/EditPositionDialog.qml</file>
<file alias="QGroundControl/Controls/ExclusiveGroupItem.qml">src/QmlControls/ExclusiveGroupItem.qml</file>
...
...
@@ -89,6 +91,7 @@
<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/QGCMapPolylineVisuals.qml">src/MissionManager/QGCMapPolylineVisuals.qml</file>
<file alias="QGroundControl/Controls/QGCMouseArea.qml">src/QmlControls/QGCMouseArea.qml</file>
<file alias="QGroundControl/Controls/QGCMovableItem.qml">src/QmlControls/QGCMovableItem.qml</file>
<file alias="QGroundControl/Controls/QGCPipable.qml">src/QmlControls/QGCPipable.qml</file>
...
...
@@ -202,6 +205,7 @@
<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="CorridorScan.SettingsGroup.json">src/MissionManager/CorridorScan.SettingsGroup.json</file>
<file alias="StructureScan.SettingsGroup.json">src/MissionManager/StructureScan.SettingsGroup.json</file>
<file alias="Units.SettingsGroup.json">src/Settings/Units.SettingsGroup.json</file>
<file alias="Video.SettingsGroup.json">src/Settings/Video.SettingsGroup.json</file>
...
...
src/MissionManager/CameraCalc.h
View file @
790d03b4
...
...
@@ -49,6 +49,14 @@ public:
Fact
*
adjustedFootprintSide
(
void
)
{
return
&
_adjustedFootprintSideFact
;
}
Fact
*
adjustedFootprintFrontal
(
void
)
{
return
&
_adjustedFootprintFrontalFact
;
}
const
Fact
*
valueSetIsDistance
(
void
)
const
{
return
&
_valueSetIsDistanceFact
;
}
const
Fact
*
distanceToSurface
(
void
)
const
{
return
&
_distanceToSurfaceFact
;
}
const
Fact
*
imageDensity
(
void
)
const
{
return
&
_imageDensityFact
;
}
const
Fact
*
frontalOverlap
(
void
)
const
{
return
&
_frontalOverlapFact
;
}
const
Fact
*
sideOverlap
(
void
)
const
{
return
&
_sideOverlapFact
;
}
const
Fact
*
adjustedFootprintSide
(
void
)
const
{
return
&
_adjustedFootprintSideFact
;
}
const
Fact
*
adjustedFootprintFrontal
(
void
)
const
{
return
&
_adjustedFootprintFrontalFact
;
}
bool
isManualCamera
(
void
)
{
return
cameraName
()
==
manualCameraName
();
}
double
imageFootprintSide
(
void
)
const
{
return
_imageFootprintSide
;
}
double
imageFootprintFrontal
(
void
)
const
{
return
_imageFootprintFrontal
;
}
...
...
src/MissionManager/ComplexMissionItem.h
View file @
790d03b4
...
...
@@ -47,9 +47,9 @@ public:
static
const
char
*
jsonComplexItemTypeKey
;
signals:
void
complexDistanceChanged
(
double
complexDistance
);
void
complexDistanceChanged
(
void
);
void
greatestDistanceToChanged
(
void
);
void
additionalTimeDelayChanged
(
double
additionalTimeDelay
);
void
additionalTimeDelayChanged
(
void
);
};
#endif
src/MissionManager/CorridorScan.SettingsGroup.json
0 → 100644
View file @
790d03b4
[
{
"name"
:
"Altitude"
,
"shortDescription"
:
"Altitude for the bottom layer of the structure scan."
,
"type"
:
"double"
,
"units"
:
"m"
,
"decimalPlaces"
:
1
,
"defaultValue"
:
50
},
{
"name"
:
"CorridorWidth"
,
"shortDescription"
:
"Corridor width. Specify 0 width for a single pass scan."
,
"type"
:
"double"
,
"units"
:
"m"
,
"min"
:
0
,
"decimalPlaces"
:
1
,
"defaultValue"
:
50
},
{
"name"
:
"Trigger distance"
,
"shortDescription"
:
"Distance between each triggering of the camera. 0 specifies not camera trigger."
,
"type"
:
"double"
,
"decimalPlaces"
:
2
,
"min"
:
0
,
"units"
:
"m"
,
"defaultValue"
:
25
},
{
"name"
:
"GridSpacing"
,
"shortDescription"
:
"Amount of spacing in between parallel grid lines."
,
"type"
:
"double"
,
"decimalPlaces"
:
2
,
"min"
:
0.1
,
"units"
:
"m"
,
"defaultValue"
:
30
},
{
"name"
:
"TurnaroundDist"
,
"shortDescription"
:
"Amount of additional distance to add outside the grid area for vehicle turnaround."
,
"type"
:
"double"
,
"decimalPlaces"
:
2
,
"min"
:
0
,
"units"
:
"m"
,
"defaultValue"
:
30
}
]
src/MissionManager/CorridorScanComplexItem.cc
0 → 100644
View file @
790d03b4
This diff is collapsed.
Click to expand it.
src/MissionManager/CorridorScanComplexItem.h
0 → 100644
View file @
790d03b4
/****************************************************************************
*
* (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.
*
****************************************************************************/
#pragma once
#include "ComplexMissionItem.h"
#include "MissionItem.h"
#include "SettingsFact.h"
#include "QGCLoggingCategory.h"
#include "QGCMapPolyline.h"
#include "QGCMapPolygon.h"
#include "CameraCalc.h"
Q_DECLARE_LOGGING_CATEGORY
(
CorridorScanComplexItemLog
)
class
CorridorScanComplexItem
:
public
ComplexMissionItem
{
Q_OBJECT
public:
CorridorScanComplexItem
(
Vehicle
*
vehicle
,
QObject
*
parent
=
NULL
);
Q_PROPERTY
(
CameraCalc
*
cameraCalc
READ
cameraCalc
CONSTANT
)
Q_PROPERTY
(
QGCMapPolyline
*
corridorPolyline
READ
corridorPolyline
CONSTANT
)
Q_PROPERTY
(
QGCMapPolygon
*
corridorPolygon
READ
corridorPolygon
CONSTANT
)
Q_PROPERTY
(
Fact
*
corridorWidth
READ
corridorWidth
CONSTANT
)
Q_PROPERTY
(
int
cameraShots
READ
cameraShots
NOTIFY
cameraShotsChanged
)
Q_PROPERTY
(
double
timeBetweenShots
READ
timeBetweenShots
NOTIFY
timeBetweenShotsChanged
)
Q_PROPERTY
(
double
coveredArea
READ
coveredArea
NOTIFY
coveredAreaChanged
)
Q_PROPERTY
(
double
cameraMinTriggerInterval
MEMBER
_cameraMinTriggerInterval
NOTIFY
cameraMinTriggerIntervalChanged
)
Q_PROPERTY
(
QVariantList
transectPoints
READ
transectPoints
NOTIFY
transectPointsChanged
)
CameraCalc
*
cameraCalc
(
void
)
{
return
&
_cameraCalc
;
}
QGCMapPolyline
*
corridorPolyline
(
void
)
{
return
&
_corridorPolyline
;
}
QGCMapPolygon
*
corridorPolygon
(
void
)
{
return
&
_corridorPolygon
;
}
Fact
*
corridorWidth
(
void
)
{
return
&
_corridorWidthFact
;
}
QVariantList
transectPoints
(
void
)
{
return
_transectPoints
;
}
int
cameraShots
(
void
)
const
{
return
_cameraShots
;
}
double
timeBetweenShots
(
void
);
double
coveredArea
(
void
)
const
;
Q_INVOKABLE
void
rotateEntryPoint
(
void
);
// Overrides from ComplexMissionItem
double
complexDistance
(
void
)
const
final
{
return
_scanDistance
;
}
int
lastSequenceNumber
(
void
)
const
final
;
bool
load
(
const
QJsonObject
&
complexObject
,
int
sequenceNumber
,
QString
&
errorString
)
final
;
double
greatestDistanceTo
(
const
QGeoCoordinate
&
other
)
const
final
;
QString
mapVisualQML
(
void
)
const
final
{
return
QStringLiteral
(
"CorridorScanMapVisual.qml"
);
}
// Overrides from VisualMissionItem
bool
dirty
(
void
)
const
final
{
return
_dirty
;
}
bool
isSimpleItem
(
void
)
const
final
{
return
false
;
}
bool
isStandaloneCoordinate
(
void
)
const
final
{
return
false
;
}
bool
specifiesCoordinate
(
void
)
const
final
;
bool
specifiesAltitudeOnly
(
void
)
const
final
{
return
false
;
}
QString
commandDescription
(
void
)
const
final
{
return
tr
(
"Corridor Scan"
);
}
QString
commandName
(
void
)
const
final
{
return
tr
(
"Corridor Scan"
);
}
QString
abbreviation
(
void
)
const
final
{
return
"S"
;
}
QGeoCoordinate
coordinate
(
void
)
const
final
{
return
_coordinate
;
}
QGeoCoordinate
exitCoordinate
(
void
)
const
final
{
return
_exitCoordinate
;
}
int
sequenceNumber
(
void
)
const
final
{
return
_sequenceNumber
;
}
double
specifiedFlightSpeed
(
void
)
final
{
return
std
::
numeric_limits
<
double
>::
quiet_NaN
();
}
double
specifiedGimbalYaw
(
void
)
final
{
return
std
::
numeric_limits
<
double
>::
quiet_NaN
();
}
double
specifiedGimbalPitch
(
void
)
final
{
return
std
::
numeric_limits
<
double
>::
quiet_NaN
();
}
void
appendMissionItems
(
QList
<
MissionItem
*>&
items
,
QObject
*
missionItemParent
)
final
;
void
setMissionFlightStatus
(
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
final
;
void
applyNewAltitude
(
double
newAltitude
)
final
;
bool
coordinateHasRelativeAltitude
(
void
)
const
final
{
return
true
/*_altitudeRelative*/
;
}
bool
exitCoordinateHasRelativeAltitude
(
void
)
const
final
{
return
true
/*_altitudeRelative*/
;
}
bool
exitCoordinateSameAsEntry
(
void
)
const
final
{
return
false
;
}
void
setDirty
(
bool
dirty
)
final
;
void
setCoordinate
(
const
QGeoCoordinate
&
coordinate
)
final
{
Q_UNUSED
(
coordinate
);
}
void
setSequenceNumber
(
int
sequenceNumber
)
final
;
void
save
(
QJsonArray
&
missionItems
)
final
;
static
const
char
*
jsonComplexItemTypeValue
;
signals:
void
cameraShotsChanged
(
void
);
void
timeBetweenShotsChanged
(
void
);
void
cameraMinTriggerIntervalChanged
(
double
cameraMinTriggerInterval
);
void
altitudeRelativeChanged
(
bool
altitudeRelative
);
void
transectPointsChanged
(
void
);
void
coveredAreaChanged
(
void
);
private
slots
:
void
_setDirty
(
void
);
void
_polylineDirtyChanged
(
bool
dirty
);
void
_polylineCountChanged
(
int
count
);
void
_clearInternal
(
void
);
void
_updateCoordinateAltitudes
(
void
);
void
_signalLastSequenceNumberChanged
(
void
);
void
_rebuildCorridor
(
void
);
void
_rebuildTransects
(
void
);
private:
void
_setExitCoordinate
(
const
QGeoCoordinate
&
coordinate
);
void
_setScanDistance
(
double
scanDistance
);
void
_setCameraShots
(
int
cameraShots
);
double
_triggerDistance
(
void
)
const
;
int
_transectCount
(
void
)
const
;
void
_rebuildCorridorPolygon
(
void
);
int
_sequenceNumber
;
bool
_dirty
;
QGeoCoordinate
_coordinate
;
QGeoCoordinate
_exitCoordinate
;
QGCMapPolyline
_corridorPolyline
;
QGCMapPolygon
_corridorPolygon
;
Fact
_corridorWidthFact
;
QVariantList
_transectPoints
;
bool
_ignoreRecalc
;
double
_scanDistance
;
int
_cameraShots
;
double
_timeBetweenShots
;
double
_cameraMinTriggerInterval
;
double
_cruiseSpeed
;
CameraCalc
_cameraCalc
;
static
QMap
<
QString
,
FactMetaData
*>
_metaDataMap
;
static
const
char
*
_corridorWidthFactName
;
static
const
char
*
_jsonCameraCalcKey
;
};
src/MissionManager/CorridorScanComplexItemTest.cc
0 → 100644
View file @
790d03b4
/****************************************************************************
*
* (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 "CorridorScanComplexItemTest.h"
#include "QGCApplication.h"
CorridorScanComplexItemTest
::
CorridorScanComplexItemTest
(
void
)
:
_offlineVehicle
(
NULL
)
{
_linePoints
<<
QGeoCoordinate
(
47.633550640000003
,
-
122.08982199
)
<<
QGeoCoordinate
(
47.634129020000003
,
-
122.08887249
)
<<
QGeoCoordinate
(
47.633619320000001
,
-
122.08811074
);
}
void
CorridorScanComplexItemTest
::
init
(
void
)
{
UnitTest
::
init
();
_offlineVehicle
=
new
Vehicle
(
MAV_AUTOPILOT_PX4
,
MAV_TYPE_QUADROTOR
,
qgcApp
()
->
toolbox
()
->
firmwarePluginManager
(),
this
);
_corridorItem
=
new
CorridorScanComplexItem
(
_offlineVehicle
,
this
);
// _corridorItem->setTurnaroundDist(0); // Unit test written for no turnaround distance
_corridorItem
->
setDirty
(
false
);
_mapPolyline
=
_corridorItem
->
corridorPolyline
();
_rgSignals
[
complexDistanceChangedIndex
]
=
SIGNAL
(
complexDistanceChanged
());
_rgSignals
[
greatestDistanceToChangedIndex
]
=
SIGNAL
(
greatestDistanceToChanged
());
_rgSignals
[
additionalTimeDelayChangedIndex
]
=
SIGNAL
(
additionalTimeDelayChanged
());
_rgSignals
[
transectPointsChangedIndex
]
=
SIGNAL
(
transectPointsChanged
());
_rgSignals
[
cameraShotsChangedIndex
]
=
SIGNAL
(
cameraShotsChanged
());
_rgSignals
[
coveredAreaChangedIndex
]
=
SIGNAL
(
coveredAreaChanged
());
_rgSignals
[
timeBetweenShotsChangedIndex
]
=
SIGNAL
(
timeBetweenShotsChanged
());
_rgSignals
[
dirtyChangedIndex
]
=
SIGNAL
(
dirtyChanged
(
bool
));
_multiSpy
=
new
MultiSignalSpy
();
QCOMPARE
(
_multiSpy
->
init
(
_corridorItem
,
_rgSignals
,
_cSignals
),
true
);
_rgCorridorPolygonSignals
[
corridorPolygonPathChangedIndex
]
=
SIGNAL
(
pathChanged
());
_multiSpyCorridorPolygon
=
new
MultiSignalSpy
();
QCOMPARE
(
_multiSpyCorridorPolygon
->
init
(
_corridorItem
->
corridorPolygon
(),
_rgCorridorPolygonSignals
,
_cCorridorPolygonSignals
),
true
);
}
void
CorridorScanComplexItemTest
::
cleanup
(
void
)
{
delete
_corridorItem
;
delete
_offlineVehicle
;
delete
_multiSpy
;
}
void
CorridorScanComplexItemTest
::
_testDirty
(
void
)
{
QVERIFY
(
!
_corridorItem
->
dirty
());
_corridorItem
->
setDirty
(
false
);
QVERIFY
(
!
_corridorItem
->
dirty
());
QVERIFY
(
_multiSpy
->
checkNoSignals
());
_corridorItem
->
setDirty
(
true
);
QVERIFY
(
_corridorItem
->
dirty
());
QVERIFY
(
_multiSpy
->
checkOnlySignalByMask
(
dirtyChangedMask
));
QVERIFY
(
_multiSpy
->
pullBoolFromSignalIndex
(
dirtyChangedIndex
));
_multiSpy
->
clearAllSignals
();
_corridorItem
->
setDirty
(
false
);
QVERIFY
(
!
_corridorItem
->
dirty
());
QVERIFY
(
_multiSpy
->
checkOnlySignalByMask
(
dirtyChangedMask
));
QVERIFY
(
!
_multiSpy
->
pullBoolFromSignalIndex
(
dirtyChangedIndex
));
_multiSpy
->
clearAllSignals
();
// These facts should set dirty when changed
QList
<
Fact
*>
rgFacts
;
#if 0
rgFacts << _corridorItem->gridAltitude() << _corridorItem->gridAngle() << _corridorItem->gridSpacing() << _corridorItem->turnaroundDist() << _corridorItem->cameraTriggerDistance() <<
_corridorItem->gridAltitudeRelative() << _corridorItem->cameraTriggerInTurnaround() << _corridorItem->hoverAndCapture();
#endif
rgFacts
<<
_corridorItem
->
corridorWidth
();
foreach
(
Fact
*
fact
,
rgFacts
)
{
qDebug
()
<<
fact
->
name
();
QVERIFY
(
!
_corridorItem
->
dirty
());
if
(
fact
->
typeIsBool
())
{
fact
->
setRawValue
(
!
fact
->
rawValue
().
toBool
());
}
else
{
fact
->
setRawValue
(
fact
->
rawValue
().
toDouble
()
+
1
);
}
QVERIFY
(
_multiSpy
->
checkSignalByMask
(
dirtyChangedMask
));
QVERIFY
(
_multiSpy
->
pullBoolFromSignalIndex
(
dirtyChangedIndex
));
_corridorItem
->
setDirty
(
false
);
_multiSpy
->
clearAllSignals
();
}
rgFacts
.
clear
();
// These facts should not change dirty bit
#if 0
rgFacts << _corridorItem->groundResolution() << _corridorItem->frontalOverlap() << _corridorItem->sideOverlap() << _corridorItem->cameraSensorWidth() << _corridorItem->cameraSensorHeight() <<
_corridorItem->cameraResolutionWidth() << _corridorItem->cameraResolutionHeight() << _corridorItem->cameraFocalLength() << _corridorItem->cameraOrientationLandscape() <<
_corridorItem->fixedValueIsAltitude() << _corridorItem->camera() << _corridorItem->manualGrid();
#endif
foreach
(
Fact
*
fact
,
rgFacts
)
{
qDebug
()
<<
fact
->
name
();
QVERIFY
(
!
_corridorItem
->
dirty
());
if
(
fact
->
typeIsBool
())
{
fact
->
setRawValue
(
!
fact
->
rawValue
().
toBool
());
}
else
{
fact
->
setRawValue
(
fact
->
rawValue
().
toDouble
()
+
1
);
}
QVERIFY
(
_multiSpy
->
checkNoSignalByMask
(
dirtyChangedMask
));
QVERIFY
(
!
_corridorItem
->
dirty
());
_multiSpy
->
clearAllSignals
();
}
rgFacts
.
clear
();
}
void
CorridorScanComplexItemTest
::
_testCameraTrigger
(
void
)
{
#if 0
QCOMPARE(_corridorItem->property("cameraTrigger").toBool(), true);
// Set up a grid
for (int i=0; i<3; i++) {
_mapPolyline->appendVertex(_linePoints[i]);
}
_corridorItem->setDirty(false);
_multiSpy->clearAllSignals();
int lastSeq = _corridorItem->lastSequenceNumber();
QVERIFY(lastSeq > 0);
// Turning off camera triggering should remove two camera trigger mission items, this should trigger:
// lastSequenceNumberChanged
// dirtyChanged
_corridorItem->setProperty("cameraTrigger", false);
QVERIFY(_multiSpy->checkOnlySignalByMask(lastSequenceNumberChangedMask | dirtyChangedMask | cameraTriggerChangedMask));
QCOMPARE(_multiSpy->pullIntFromSignalIndex(lastSequenceNumberChangedIndex), lastSeq - 2);
_corridorItem->setDirty(false);
_multiSpy->clearAllSignals();
// Turn on camera triggering and make sure things go back to previous count
_corridorItem->setProperty("cameraTrigger", true);
QVERIFY(_multiSpy->checkOnlySignalByMask(lastSequenceNumberChangedMask | dirtyChangedMask | cameraTriggerChangedMask));
QCOMPARE(_multiSpy->pullIntFromSignalIndex(lastSequenceNumberChangedIndex), lastSeq);
#endif
}
void
CorridorScanComplexItemTest
::
_setPolyline
(
void
)
{
for
(
int
i
=
0
;
i
<
_linePoints
.
count
();
i
++
)
{
QGeoCoordinate
&
vertex
=
_linePoints
[
i
];
_mapPolyline
->
appendVertex
(
vertex
);
}
}
#if 0
void CorridorScanComplexItemTest::_testEntryLocation(void)
{
_setPolygon();
for (double gridAngle=-360.0; gridAngle<=360.0; gridAngle++) {
_corridorItem->gridAngle()->setRawValue(gridAngle);
QList<QGeoCoordinate> rgSeenEntryCoords;
QList<int> rgEntryLocation;
rgEntryLocation << SurveyMissionItem::EntryLocationTopLeft
<< SurveyMissionItem::EntryLocationTopRight
<< SurveyMissionItem::EntryLocationBottomLeft
<< SurveyMissionItem::EntryLocationBottomRight;
// Validate that each entry location is unique
for (int i=0; i<rgEntryLocation.count(); i++) {
int entryLocation = rgEntryLocation[i];
_corridorItem->gridEntryLocation()->setRawValue(entryLocation);
QVERIFY(!rgSeenEntryCoords.contains(_corridorItem->coordinate()));
rgSeenEntryCoords << _corridorItem->coordinate();
}
rgSeenEntryCoords.clear();
}
}
#endif
void
CorridorScanComplexItemTest
::
_testItemCount
(
void
)
{
QList
<
MissionItem
*>
items
;
_setPolyline
();
// _corridorItem->cameraTriggerInTurnaround()->setRawValue(false);
_corridorItem
->
appendMissionItems
(
items
,
this
);
QCOMPARE
(
items
.
count
(),
_corridorItem
->
lastSequenceNumber
());
items
.
clear
();
}
void
CorridorScanComplexItemTest
::
_testPathChanges
(
void
)
{
_setPolyline
();
_corridorItem
->
setDirty
(
false
);
_multiSpy
->
clearAllSignals
();
_multiSpyCorridorPolygon
->
clearAllSignals
();
QGeoCoordinate
vertex
=
_mapPolyline
->
vertexCoordinate
(
1
);
vertex
.
setLatitude
(
vertex
.
latitude
()
+
0.01
);
_mapPolyline
->
adjustVertex
(
1
,
vertex
);
QVERIFY
(
_corridorItem
->
dirty
());
QVERIFY
(
_multiSpy
->
checkOnlySignalsByMask
(
dirtyChangedMask
|
transectPointsChangedMask
|
cameraShotsChangedMask
|
coveredAreaChangedMask
|
complexDistanceChangedMask
|
greatestDistanceToChangedMask
));
QVERIFY
(
_multiSpyCorridorPolygon
->
checkSignalsByMask
(
corridorPolygonPathChangedMask
));
_multiSpy
->
clearAllSignals
();
}
src/MissionManager/CorridorScanComplexItemTest.h
0 → 100644
View file @
790d03b4
/****************************************************************************
*
* (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.
*
****************************************************************************/
#pragma once
#include "UnitTest.h"
#include "TCPLink.h"
#include "MultiSignalSpy.h"
#include "CorridorScanComplexItem.h"
#include <QGeoCoordinate>
class
CorridorScanComplexItemTest
:
public
UnitTest
{
Q_OBJECT
public:
CorridorScanComplexItemTest
(
void
);
protected:
void
init
(
void
)
final
;
void
cleanup
(
void
)
final
;
private
slots
:
void
_testDirty
(
void
);
void
_testCameraTrigger
(
void
);
// void _testEntryLocation(void);
void
_testItemCount
(
void
);
void
_testPathChanges
(
void
);
private:
void
_setPolyline
(
void
);
enum
{
complexDistanceChangedIndex
=
0
,
greatestDistanceToChangedIndex
,
additionalTimeDelayChangedIndex
,
transectPointsChangedIndex
,
cameraShotsChangedIndex
,
coveredAreaChangedIndex
,
timeBetweenShotsChangedIndex
,
dirtyChangedIndex
,
maxSignalIndex
};
enum
{
complexDistanceChangedMask
=
1
<<
complexDistanceChangedIndex
,
greatestDistanceToChangedMask
=
1
<<
greatestDistanceToChangedIndex
,
additionalTimeDelayChangedMask
=
1
<<
additionalTimeDelayChangedIndex
,
transectPointsChangedMask
=
1
<<
transectPointsChangedIndex
,
cameraShotsChangedMask
=
1
<<
cameraShotsChangedIndex
,
coveredAreaChangedMask
=
1
<<
coveredAreaChangedIndex
,
timeBetweenShotsChangedMask
=
1
<<
timeBetweenShotsChangedIndex
,
dirtyChangedMask
=
1
<<
dirtyChangedIndex
};
static
const
size_t
_cSignals
=
maxSignalIndex
;
const
char
*
_rgSignals
[
_cSignals
];
enum
{
corridorPolygonPathChangedIndex
=
0
,
maxCorridorPolygonSignalIndex
};
enum
{
corridorPolygonPathChangedMask
=
1
<<
corridorPolygonPathChangedIndex
,
};
static
const
size_t
_cCorridorPolygonSignals
=
maxCorridorPolygonSignalIndex
;
const
char
*
_rgCorridorPolygonSignals
[
_cCorridorPolygonSignals
];
Vehicle
*
_offlineVehicle
;
MultiSignalSpy
*
_multiSpy
;
MultiSignalSpy
*
_multiSpyCorridorPolygon
;
CorridorScanComplexItem
*
_corridorItem
;
QGCMapPolyline
*
_mapPolyline
;
QList
<
QGeoCoordinate
>
_linePoints
;
};
src/MissionManager/MissionController.cc
View file @
790d03b4
...
...
@@ -19,7 +19,7 @@
#include "SurveyMissionItem.h"
#include "FixedWingLandingComplexItem.h"
#include "StructureScanComplexItem.h"
#include "
Structure
ScanComplexItem.h"
#include "
Corridor
ScanComplexItem.h"
#include "JsonHelper.h"
#include "ParameterManager.h"
#include "QGroundControlQmlGlobal.h"
...
...
@@ -63,6 +63,7 @@ MissionController::MissionController(PlanMasterController* masterController, QOb
,
_surveyMissionItemName
(
tr
(
"Survey"
))
,
_fwLandingMissionItemName
(
tr
(
"Fixed Wing Landing"
))
,
_structureScanMissionItemName
(
tr
(
"Structure Scan"
))
,
_corridorScanMissionItemName
(
tr
(
"Corridor Scan"
))
,
_appSettings
(
qgcApp
()
->
toolbox
()
->
settingsManager
()
->
appSettings
())
,
_progressPct
(
0
)
,
_currentPlanViewIndex
(
-
1
)
...
...
@@ -416,6 +417,8 @@ int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate
newItem
=
new
FixedWingLandingComplexItem
(
_controllerVehicle
,
_visualItems
);
}
else
if
(
itemName
==
_structureScanMissionItemName
)
{
newItem
=
new
StructureScanComplexItem
(
_controllerVehicle
,
_visualItems
);
}
else
if
(
itemName
==
_corridorScanMissionItemName
)
{
newItem
=
new
CorridorScanComplexItem
(
_controllerVehicle
,
_visualItems
);
}
else
{
qWarning
()
<<
"Internal error: Unknown complex item:"
<<
itemName
;
return
sequenceNumber
;
...
...
@@ -697,6 +700,15 @@ bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjec
nextSequenceNumber
=
structureItem
->
lastSequenceNumber
()
+
1
;
qCDebug
(
MissionControllerLog
)
<<
"Structure Scan load complete: nextSequenceNumber"
<<
nextSequenceNumber
;
visualItems
->
append
(
structureItem
);
}
else
if
(
complexItemType
==
CorridorScanComplexItem
::
jsonComplexItemTypeValue
)
{
qCDebug
(
MissionControllerLog
)
<<
"Loading Corridor Scan: nextSequenceNumber"
<<
nextSequenceNumber
;
CorridorScanComplexItem
*
corridorItem
=
new
CorridorScanComplexItem
(
_controllerVehicle
,
visualItems
);
if
(
!
corridorItem
->
load
(
itemObject
,
nextSequenceNumber
++
,
errorString
))
{
return
false
;
}
nextSequenceNumber
=
corridorItem
->
lastSequenceNumber
()
+
1
;
qCDebug
(
MissionControllerLog
)
<<
"Corridor Scan load complete: nextSequenceNumber"
<<
nextSequenceNumber
;
visualItems
->
append
(
corridorItem
);
}
else
if
(
complexItemType
==
MissionSettingsItem
::
jsonComplexItemTypeValue
)
{
qCDebug
(
MissionControllerLog
)
<<
"Loading Mission Settings: nextSequenceNumber"
<<
nextSequenceNumber
;
MissionSettingsItem
*
settingsItem
=
new
MissionSettingsItem
(
_controllerVehicle
,
visualItems
);
...
...
@@ -1841,6 +1853,7 @@ QStringList MissionController::complexMissionItemNames(void) const
QStringList
complexItems
;
complexItems
.
append
(
_surveyMissionItemName
);
complexItems
.
append
(
_corridorScanMissionItemName
);
if
(
_controllerVehicle
->
fixedWing
())
{
complexItems
.
append
(
_fwLandingMissionItemName
);
}
...
...
src/MissionManager/MissionController.h
View file @
790d03b4
...
...
@@ -253,6 +253,7 @@ private:
QString
_surveyMissionItemName
;
QString
_fwLandingMissionItemName
;
QString
_structureScanMissionItemName
;
QString
_corridorScanMissionItemName
;
AppSettings
*
_appSettings
;
double
_progressPct
;
int
_currentPlanViewIndex
;
...
...
src/MissionManager/QGCMapPolygon.cc
View file @
790d03b4
...
...
@@ -362,7 +362,7 @@ QGeoCoordinate QGCMapPolygon::vertexCoordinate(int vertex) const
}
}
QList
<
QPointF
>
QGCMapPolygon
::
nedPolygon
(
void
)
QList
<
QPointF
>
QGCMapPolygon
::
nedPolygon
(
void
)
const
{
QList
<
QPointF
>
nedPolygon
;
...
...
@@ -515,3 +515,19 @@ bool QGCMapPolygon::loadKMLFile(const QString& kmlFile)
return
true
;
}
double
QGCMapPolygon
::
area
(
void
)
const