Skip to content

Q
qgroundcontrol
Commit 842ba9eb authored by DoinLakeFlyer's avatar DoinLakeFlyer
Browse files
Options 

Survey And Corridor now share same transect generation code. Better unit tests for both with more coverage.
parent 127271cd
Show whitespace changes
Inline Side-by-side
Showing with 516 additions and 495 deletions
qgroundcontrol.pro
View file @ 842ba9eb
......@@ -491,6 +491,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin {
src/MissionManager/StructureScanComplexItemTest.h \
src/MissionManager/SurveyComplexItemTest.h \
src/MissionManager/TransectStyleComplexItemTest.h \
src/MissionManager/TransectStyleComplexItemTestBase.h \
src/MissionManager/VisualMissionItemTest.h \
src/qgcunittest/GeoTest.h \
src/qgcunittest/LinkManagerTest.h \
......@@ -533,6 +534,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin {
src/MissionManager/StructureScanComplexItemTest.cc \
src/MissionManager/SurveyComplexItemTest.cc \
src/MissionManager/TransectStyleComplexItemTest.cc \
src/MissionManager/TransectStyleComplexItemTestBase.cc \
src/MissionManager/VisualMissionItemTest.cc \
src/qgcunittest/GeoTest.cc \
src/qgcunittest/LinkManagerTest.cc \
......
src/MissionManager/CorridorScanComplexItem.cc
View file @ 842ba9eb
......@@ -143,140 +143,10 @@ bool CorridorScanComplexItem::specifiesCoordinate(void) const
return _corridorPolyline.count() > 1;
}
int CorridorScanComplexItem::_transectCount(void) const
int CorridorScanComplexItem::_calcTransectCount(void) const
{
double fullWidth = _corridorWidthFact.rawValue().toDouble();
return fullWidth > 0.0 ? qCeil(fullWidth / _transectSpacing()) : 1;
}
void CorridorScanComplexItem::_appendLoadedMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
qCDebug(CorridorScanComplexItemLog) << "_appendLoadedMissionItems";
int seqNum = _sequenceNumber;
for (const MissionItem* loadedMissionItem: _loadedMissionItems) {
MissionItem* item = new MissionItem(*loadedMissionItem, missionItemParent);
item->setSequenceNumber(seqNum++);
items.append(item);
}
}
void CorridorScanComplexItem::_buildAndAppendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
qCDebug(CorridorScanComplexItemLog) << "_buildAndAppendMissionItems";
// Now build the mission items from the transect points
MissionItem* item;
int seqNum = _sequenceNumber;
bool imagesEverywhere = _cameraTriggerInTurnAroundFact.rawValue().toBool();
bool addTriggerAtBeginning = imagesEverywhere;
bool firstOverallPoint = true;
MAV_FRAME mavFrame = followTerrain() || !_cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT;
//qDebug() << "_buildAndAppendMissionItems";
for (const QList<TransectStyleComplexItem::CoordInfo_t>& transect: _transects) {
bool transectEntry = true;
//qDebug() << "start transect";
for (const CoordInfo_t& transectCoordInfo: transect) {
//qDebug() << transectCoordInfo.coordType;
item = new MissionItem(seqNum++,
MAV_CMD_NAV_WAYPOINT,
mavFrame,
0, // No hold time
0.0, // No acceptance radius specified
0.0, // Pass through waypoint
std::numeric_limits<double>::quiet_NaN(), // Yaw unchanged
transectCoordInfo.coord.latitude(),
transectCoordInfo.coord.longitude(),
transectCoordInfo.coord.altitude(),
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
if (triggerCamera() && firstOverallPoint && addTriggerAtBeginning) {
// Start triggering
addTriggerAtBeginning = false;
item = new MissionItem(seqNum++,
MAV_CMD_DO_SET_CAM_TRIGG_DIST,
MAV_FRAME_MISSION,
_cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble(), // trigger distance
0, // shutter integration (ignore)
1, // trigger immediately when starting
0, 0, 0, 0, // param 4-7 unused
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
}
firstOverallPoint = false;
// Possibly add trigger start/stop to survey area entrance/exit
if (triggerCamera() && (transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEntry || transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyExit)) {
if (transectEntry) {
// Start of transect, always start triggering. We do this even if we are taking images everywhere.
// This allows a restart of the mission in mid-air without losing images from the entire mission.
// At most you may lose part of a transect.
item = new MissionItem(seqNum++,
MAV_CMD_DO_SET_CAM_TRIGG_DIST,
MAV_FRAME_MISSION,
_cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble(), // trigger distance
0, // shutter integration (ignore)
1, // trigger immediately when starting
0, 0, 0, 0, // param 4-7 unused
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
transectEntry = false;
} else if (!imagesEverywhere && !transectEntry){
// End of transect, stop triggering
item = new MissionItem(seqNum++,
MAV_CMD_DO_SET_CAM_TRIGG_DIST,
MAV_FRAME_MISSION,
0, // stop triggering
0, // shutter integration (ignore)
0, // trigger immediately when starting
0, 0, 0, 0, // param 4-7 unused
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
}
}
}
}
if (imagesEverywhere) {
// Stop triggering
MissionItem* item = new MissionItem(seqNum++,
MAV_CMD_DO_SET_CAM_TRIGG_DIST,
MAV_FRAME_MISSION,
0, // stop triggering
0, // shutter integration (ignore)
0, // trigger immediately when starting
0, 0, 0, 0, // param 4-7 unused
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
}
}
void CorridorScanComplexItem::appendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
if (_loadedMissionItems.count()) {
// We have mission items from the loaded plan, use those
_appendLoadedMissionItems(items, missionItemParent);
} else {
// Build the mission items on the fly
_buildAndAppendMissionItems(items, missionItemParent);
}
return fullWidth > 0.0 ? qCeil(fullWidth / _calcTransectSpacing()) : 1;
}
void CorridorScanComplexItem::applyNewAltitude(double newAltitude)
......@@ -343,10 +213,10 @@ void CorridorScanComplexItem::_rebuildTransectsPhase1(void)
_transects.clear();
_transectsPathHeightInfo.clear();
double transectSpacing = _transectSpacing();
double transectSpacing = _calcTransectSpacing();
double fullWidth = _corridorWidthFact.rawValue().toDouble();
double halfWidth = fullWidth / 2.0;
int transectCount = _transectCount();
int transectCount = _calcTransectCount();
double normalizedTransectPosition = transectSpacing / 2.0;
if (_corridorPolyline.count() >= 2) {
......@@ -486,7 +356,7 @@ void CorridorScanComplexItem::_recalcCameraShots(void)
_cameraShots = qCeil(_complexDistance / triggerDistance);
} else {
int singleTransectImageCount = qCeil(_corridorPolyline.length() / triggerDistance);
_cameraShots = singleTransectImageCount * _transectCount();
_cameraShots = singleTransectImageCount * _calcTransectCount();
}
}
emit cameraShotsChanged();
......@@ -502,7 +372,7 @@ double CorridorScanComplexItem::timeBetweenShots(void)
return _cruiseSpeed == 0 ? 0 : _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / _cruiseSpeed;
}
double CorridorScanComplexItem::_transectSpacing(void) const
double CorridorScanComplexItem::_calcTransectSpacing(void) const
{
double transectSpacing = _cameraCalc.adjustedFootprintSide()->rawValue().toDouble();
if (transectSpacing < 0.5) {
......
src/MissionManager/CorridorScanComplexItem.h
View file @ 842ba9eb
......@@ -38,7 +38,6 @@ public:
// Overrides from TransectStyleComplexItem
void save (QJsonArray& planItems) final;
bool specifiesCoordinate (void) const final;
void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final;
void applyNewAltitude (double newAltitude) final;
double timeBetweenShots (void) final;
......@@ -68,10 +67,8 @@ private slots:
void _recalcCameraShots (void) final;
private:
double _transectSpacing (void) const;
int _transectCount (void) const;
void _buildAndAppendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent);
void _appendLoadedMissionItems (QList<MissionItem*>& items, QObject* missionItemParent);
double _calcTransectSpacing (void) const;
int _calcTransectCount (void) const;
QGCMapPolyline _corridorPolyline;
QList<QList<QGeoCoordinate>> _transectSegments; ///< Internal transect segments including grid exit, turnaround and internal camera points
......
src/MissionManager/CorridorScanComplexItemTest.cc
View file @ 842ba9eb
......@@ -12,25 +12,31 @@
CorridorScanComplexItemTest::CorridorScanComplexItemTest(void)
{
_linePoints << QGeoCoordinate(47.633550640000003, -122.08982199)
<< QGeoCoordinate(47.634129020000003, -122.08887249)
<< QGeoCoordinate(47.633619320000001, -122.08811074);
_polyLineVertices.append(QGeoCoordinate(47.633550640000003, -122.08982199));
_polyLineVertices.append(_polyLineVertices[0].atDistanceAndAzimuth(_corridorLineSegmentDistance, 0));
_polyLineVertices.append(_polyLineVertices[1].atDistanceAndAzimuth(_corridorLineSegmentDistance, 20));
}
void CorridorScanComplexItemTest::init(void)
{
UnitTest::init();
TransectStyleComplexItemTestBase::init();
_masterController = new PlanMasterController(this);
_controllerVehicle = _masterController->controllerVehicle();
_corridorItem = new CorridorScanComplexItem(_masterController, false /* flyView */, QString() /* kmlFile */, this /* parent */);
_corridorItem->corridorPolyline()->appendVertices(_polyLineVertices);
// Setup for expected transect count
_corridorItem->corridorWidth()->setRawValue(_corridorWidth);
_corridorItem->cameraCalc()->adjustedFootprintSide()->setRawValue((_corridorWidth * 0.5) + 1.0);
_corridorItem->cameraCalc()->adjustedFootprintFrontal()->setRawValue(_corridorLineSegmentDistance * 0.25);
int expectedTransectCount = _expectedTransectCount;
QCOMPARE(_corridorItem->_transectCount(), expectedTransectCount);
// vehicleSpeed need for terrain calcs
MissionController::MissionFlightStatus_t missionFlightStatus;
missionFlightStatus.vehicleSpeed = 5;
_corridorItem->setMissionFlightStatus(missionFlightStatus);
_setPolyline();
_corridorItem->setDirty(false);
_rgCorridorPolygonSignals[corridorPolygonPathChangedIndex] = SIGNAL(pathChanged());
......@@ -42,6 +48,7 @@ void CorridorScanComplexItemTest::init(void)
void CorridorScanComplexItemTest::cleanup(void)
{
delete _corridorItem;
TransectStyleComplexItemTestBase::cleanup();
}
void CorridorScanComplexItemTest::_testDirty(void)
......@@ -98,14 +105,6 @@ void CorridorScanComplexItemTest::_testCameraTrigger(void)
#endif
}
void CorridorScanComplexItemTest::_setPolyline(void)
{
for (int i=0; i<_linePoints.count(); i++) {
QGeoCoordinate& vertex = _linePoints[i];
_corridorItem->corridorPolyline()->appendVertex(vertex);
}
}
#if 0
void CorridorScanComplexItemTest::_testEntryLocation(void)
{
......@@ -148,71 +147,111 @@ void CorridorScanComplexItemTest::_waitForReadyForSave(void)
void CorridorScanComplexItemTest::_testItemCount(void)
{
QList<MissionItem*> items;
typedef struct {
bool triggerInTurnAround;
bool hasTurnaround;
} TestCase_t;
static const TestCase_t rgTestCases[] = {
{ false, false },
{ false, false },
{ false, true },
{ false, true },
};
_corridorItem->turnAroundDistance()->setRawValue(0);
_corridorItem->cameraTriggerInTurnAround()->setRawValue(true);
QList<MissionItem*> items;
for (const TestCase_t& testCase : rgTestCases) {
qDebug() << "triggerInTurnAround:hasTurnaround" << testCase.triggerInTurnAround << testCase.hasTurnaround;
_corridorItem->cameraTriggerInTurnAround()->setRawValue(testCase.triggerInTurnAround);
_corridorItem->turnAroundDistance()->setRawValue(testCase.hasTurnaround ? 50 : 0);
_corridorItem->appendMissionItems(items, this);
QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber());
items.clear();
}
}
_corridorItem->turnAroundDistance()->setRawValue(0);
_corridorItem->cameraTriggerInTurnAround()->setRawValue(false);
_corridorItem->appendMissionItems(items, this);
QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber());
items.clear();
void CorridorScanComplexItemTest::_testPathChanges(void)
{
QGeoCoordinate vertex = _corridorItem->corridorPolyline()->vertexCoordinate(1);
vertex.setLatitude(vertex.latitude() + 0.01);
_corridorItem->corridorPolyline()->adjustVertex(1, vertex);
_corridorItem->turnAroundDistance()->setRawValue(20);
_corridorItem->cameraTriggerInTurnAround()->setRawValue(true);
_corridorItem->appendMissionItems(items, this);
QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber());
items.clear();
QVERIFY(_multiSpyCorridorPolygon->checkSignalsByMask(corridorPolygonPathChangedMask));
}
_corridorItem->turnAroundDistance()->setRawValue(20);
_corridorItem->cameraTriggerInTurnAround()->setRawValue(false);
_corridorItem->appendMissionItems(items, this);
QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber());
items.clear();
QList<MAV_CMD> CorridorScanComplexItemTest::_createExpectedCommands(bool hasTurnaround, bool useConditionGate)
{
static const QList<MAV_CMD> singleFullTransect = {
MAV_CMD_NAV_WAYPOINT, // Turnaround
MAV_CMD_CONDITION_GATE, // Survey area entry edge
MAV_CMD_DO_SET_CAM_TRIGG_DIST,
MAV_CMD_NAV_WAYPOINT, // Polyline turn
MAV_CMD_CONDITION_GATE, // Survey area exit edge
MAV_CMD_DO_SET_CAM_TRIGG_DIST,
MAV_CMD_NAV_WAYPOINT, // Turnaround
};
QList<MAV_CMD> singleTransect = singleFullTransect;
QList<MAV_CMD> expectedCommands;
if (!useConditionGate) {
for (MAV_CMD& cmd : singleTransect) {
cmd = cmd == MAV_CMD_CONDITION_GATE ? MAV_CMD_NAV_WAYPOINT : cmd;
}
}
#if 0
// Terrain queries seem to take random amount of time so these don't work 100%
_corridorItem->setFollowTerrain(true);
if (!hasTurnaround) {
singleTransect.takeFirst();
singleTransect.takeLast();
}
_corridorItem->turnAroundDistance()->setRawValue(0);
_corridorItem->cameraTriggerInTurnAround()->setRawValue(true);
_waitForReadyForSave();
_corridorItem->appendMissionItems(items, this);
QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber());
items.clear();
for (int i=0; i<_expectedTransectCount; i++) {
expectedCommands.append(singleTransect);
}
_corridorItem->turnAroundDistance()->setRawValue(0);
_corridorItem->cameraTriggerInTurnAround()->setRawValue(false);
_waitForReadyForSave();
_corridorItem->appendMissionItems(items, this);
QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber());
items.clear();
return expectedCommands;
}
_corridorItem->turnAroundDistance()->setRawValue(20);
_corridorItem->cameraTriggerInTurnAround()->setRawValue(true);
_waitForReadyForSave();
_corridorItem->appendMissionItems(items, this);
QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber());
items.clear();
_corridorItem->turnAroundDistance()->setRawValue(20);
_corridorItem->cameraTriggerInTurnAround()->setRawValue(false);
_waitForReadyForSave();
void CorridorScanComplexItemTest::_testItemGenerationWorker(bool imagesInTurnaround, bool hasTurnaround, bool useConditionGate, const QList<MAV_CMD>& expectedCommands)
{
qDebug() << QStringLiteral("_testItemGenerationWorker imagesInTuraround:%1 turnaround:%2 gate:%3").arg(imagesInTurnaround).arg(hasTurnaround).arg(useConditionGate);
_corridorItem->turnAroundDistance()->setRawValue(hasTurnaround ? 50 : 0);
_corridorItem->cameraTriggerInTurnAround()->setRawValue(imagesInTurnaround);
_planViewSettings->useConditionGate()->setRawValue(useConditionGate);
QList<MissionItem*> items;
_corridorItem->appendMissionItems(items, this);
QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber());
items.clear();
#endif
//_printItemCommands(items);
QCOMPARE(items.count(), expectedCommands.count());
for (int i=0; i<expectedCommands.count(); i++) {
int actualCommand = items[i]->command();
int expectedCommand = expectedCommands[i];
//qDebug() << "Index" << i;
QCOMPARE(actualCommand, expectedCommand);
}
}
void CorridorScanComplexItemTest::_testPathChanges(void)
void CorridorScanComplexItemTest::_testItemGeneration(void)
{
QGeoCoordinate vertex = _corridorItem->corridorPolyline()->vertexCoordinate(1);
vertex.setLatitude(vertex.latitude() + 0.01);
_corridorItem->corridorPolyline()->adjustVertex(1, vertex);
QVERIFY(_multiSpyCorridorPolygon->checkSignalsByMask(corridorPolygonPathChangedMask));
// Test all the combinations of: cameraTriggerInTurnAround: false, hasTurnAround: *, useConditionGate: *
typedef struct {
bool hasTurnaround;
bool useConditionGate;
} TestCase_t;
static const TestCase_t rgTestCases[] = {
{ false, false },
{ false, true },
{ true, false },
{ true, true },
};
for (const TestCase_t& testCase : rgTestCases) {
_testItemGenerationWorker(false /* imagesInTurnaround */, testCase.hasTurnaround, testCase.useConditionGate, _createExpectedCommands(testCase.hasTurnaround, testCase.useConditionGate));
}
}
src/MissionManager/CorridorScanComplexItemTest.h
View file @ 842ba9eb
......@@ -9,7 +9,7 @@
#pragma once
#include "UnitTest.h"
#include "TransectStyleComplexItemTestBase.h"
#include "TCPLink.h"
#include "MultiSignalSpy.h"
#include "CorridorScanComplexItem.h"
......@@ -17,7 +17,7 @@
#include <QGeoCoordinate>
class CorridorScanComplexItemTest : public UnitTest
class CorridorScanComplexItemTest : public TransectStyleComplexItemTestBase
{
Q_OBJECT
......@@ -25,19 +25,31 @@ public:
CorridorScanComplexItemTest(void);
protected:
void init(void) final;
void init (void) final;
void cleanup(void) final;
#if 1
private slots:
void _testDirty(void);
void _testCameraTrigger(void);
// void _testEntryLocation(void);
void _testItemCount(void);
void _testPathChanges(void);
void _testDirty (void);
void _testCameraTrigger (void);
void _testPathChanges (void);
void _testItemGeneration(void);
void _testItemCount (void);
#else
// Used to debug a single test
private slots:
void _testItemGeneration(void);
private:
void _testDirty (void);
void _testCameraTrigger (void);
void _testPathChanges (void);
void _testItemCount (void);
#endif
private:
void _setPolyline(void);
void _waitForReadyForSave(void);
QList<MAV_CMD> _createExpectedCommands(bool hasTurnaround, bool useConditionGate);
void _testItemGenerationWorker(bool imagesInTurnaround, bool hasTurnaround, bool useConditionGate, const QList<MAV_CMD>& expectedCommands);
enum {
corridorPolygonPathChangedIndex = 0,
......@@ -51,9 +63,11 @@ private:
static const size_t _cCorridorPolygonSignals = maxCorridorPolygonSignalIndex;
const char* _rgCorridorPolygonSignals[_cCorridorPolygonSignals];
PlanMasterController* _masterController = nullptr;
Vehicle* _controllerVehicle = nullptr;
MultiSignalSpy* _multiSpyCorridorPolygon = nullptr;
CorridorScanComplexItem* _corridorItem = nullptr;
QList<QGeoCoordinate> _linePoints;
QList<QGeoCoordinate> _polyLineVertices;
static constexpr int _expectedTransectCount = 2;
static constexpr double _corridorLineSegmentDistance = 200.0;
static constexpr double _corridorWidth = 50.0;
};
src/MissionManager/SurveyComplexItem.cc
View file @ 842ba9eb
......@@ -653,159 +653,9 @@ bool SurveyComplexItem::_nextTransectCoord(const QList<QGeoCoordinate>& transect
return true;
}
void SurveyComplexItem::_appendWaypoint(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, float holdTime, const QGeoCoordinate& coordinate)
{
MissionItem* item = new MissionItem(seqNum++,
MAV_CMD_NAV_WAYPOINT,
mavFrame,
holdTime,
0.0, // No acceptance radius specified
0.0, // Pass through waypoint
std::numeric_limits<double>::quiet_NaN(), // Yaw unchanged
coordinate.latitude(),
coordinate.longitude(),
coordinate.altitude(),
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
}
void SurveyComplexItem::_appendSinglePhotoCapture(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum)
{
MissionItem* item = new MissionItem(seqNum++,
MAV_CMD_IMAGE_START_CAPTURE,
MAV_FRAME_MISSION,
0, // Reserved (Set to 0)
0, // Interval (none)
1, // Take 1 photo
qQNaN(), qQNaN(), qQNaN(), qQNaN(), // param 4-7 reserved
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
}
void SurveyComplexItem::_appendConditionGate(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate)
{
MissionItem* item = new MissionItem(seqNum++,
MAV_CMD_CONDITION_GATE,
mavFrame,
0, // Gate is orthogonal to path
0, // Ignore altitude
0, 0, // Param 3-4 ignored
coordinate.latitude(),
coordinate.longitude(),
0, // No altitude
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
}
void SurveyComplexItem::_appendCameraTriggerDistance(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, float triggerDistance)
{
MissionItem* item = new MissionItem(seqNum++,
MAV_CMD_DO_SET_CAM_TRIGG_DIST,
MAV_FRAME_MISSION,
triggerDistance,
0, // shutter integration (ignore)
1, // 1 - trigger one image immediately, both and entry and exit to get full coverage
0, 0, 0, 0, // param 4-7 unused
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
}
void SurveyComplexItem::_appendCameraTriggerDistanceUpdatePoint(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate, bool useConditionGate, float triggerDistance)
{
if (useConditionGate) {
_appendConditionGate(items, missionItemParent, seqNum, mavFrame, coordinate);
} else {
_appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, coordinate);
}
_appendCameraTriggerDistance(items, missionItemParent, seqNum, triggerDistance);
}
void SurveyComplexItem::_buildAndAppendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
qCDebug(SurveyComplexItemLog) << "_buildAndAppendMissionItems";
// Now build the mission items from the transect points
int seqNum = _sequenceNumber;
bool imagesInTurnaround = _cameraTriggerInTurnAroundFact.rawValue().toBool() && turnAroundDistance()->rawValue().toDouble() != 0;
bool addTriggerAtBeginningEnd = !hoverAndCaptureEnabled() && imagesInTurnaround && triggerCamera();
bool useConditionGate = _controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(MAV_CMD_CONDITION_GATE) &&
triggerCamera() &&
!hoverAndCaptureEnabled();
MAV_FRAME mavFrame = followTerrain() || !_cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT;
// Note: The code below is written to be understable as oppose to being compact and/or remove duplicate code
int transectIndex = 0;
for (const QList<TransectStyleComplexItem::CoordInfo_t>& transect: _transects) {
bool entryTurnaround = true;
for (const CoordInfo_t& transectCoordInfo: transect) {
switch (transectCoordInfo.coordType) {
case CoordTypeTurnaround:
{
bool firstEntryTurnaround = transectIndex == 0 && entryTurnaround;
bool lastExitTurnaround = transectIndex == _transects.count() - 1 && !entryTurnaround;
if (addTriggerAtBeginningEnd && (firstEntryTurnaround || lastExitTurnaround)) {
_appendCameraTriggerDistanceUpdatePoint(items, missionItemParent, seqNum, mavFrame, transectCoordInfo.coord, useConditionGate, firstEntryTurnaround ? triggerDistance() : 0);
} else {
_appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord);
}
entryTurnaround = false;
}
break;
case CoordTypeInterior:
case CoordTypeInteriorTerrainAdded:
_appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord);
break;
case CoordTypeInteriorHoverTrigger:
_appendWaypoint(items, missionItemParent, seqNum, mavFrame, _hoverAndCaptureDelaySeconds, transectCoordInfo.coord);
_appendSinglePhotoCapture(items, missionItemParent, seqNum);
break;
case CoordTypeSurveyEntry:
if (triggerCamera()) {
if (hoverAndCaptureEnabled()) {
_appendWaypoint(items, missionItemParent, seqNum, mavFrame, _hoverAndCaptureDelaySeconds, transectCoordInfo.coord);
_appendSinglePhotoCapture(items, missionItemParent, seqNum);
} else {
// We always add a trigger start to survey entry. Even for imagesInTurnaround = true. This allows you to resume a mission and refly a transect
_appendCameraTriggerDistanceUpdatePoint(items, missionItemParent, seqNum, mavFrame, transectCoordInfo.coord, useConditionGate, triggerDistance());
}
} else {
_appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord);
}
break;
case CoordTypeSurveyExit:
if (triggerCamera()) {
if (hoverAndCaptureEnabled()) {
_appendWaypoint(items, missionItemParent, seqNum, mavFrame, _hoverAndCaptureDelaySeconds, transectCoordInfo.coord);
_appendSinglePhotoCapture(items, missionItemParent, seqNum);
} else if (imagesInTurnaround) {
_appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord);
} else {
_appendCameraTriggerDistanceUpdatePoint(items, missionItemParent, seqNum, mavFrame, transectCoordInfo.coord, useConditionGate, 0 /* triggerDistance */);
}
} else {
_appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord);
}
break;
}
}
transectIndex++;
}
}
bool SurveyComplexItem::_hasTurnaround(void) const
{
return _turnaroundDistance() > 0;
return _turnAroundDistance() > 0;
}
double SurveyComplexItem::_turnaroundDistance(void) const
......@@ -1548,30 +1398,6 @@ SurveyComplexItem::ReadyForSaveState SurveyComplexItem::readyForSaveState(void)
return TransectStyleComplexItem::readyForSaveState();
}
void SurveyComplexItem::appendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
if (_loadedMissionItems.count()) {
// We have mission items from the loaded plan, use those
_appendLoadedMissionItems(items, missionItemParent);
} else {
// Build the mission items on the fly
_buildAndAppendMissionItems(items, missionItemParent);
}
}
void SurveyComplexItem::_appendLoadedMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
qCDebug(SurveyComplexItemLog) << "_appendLoadedMissionItems";
int seqNum = _sequenceNumber;
for (const MissionItem* loadedMissionItem: _loadedMissionItems) {
MissionItem* item = new MissionItem(*loadedMissionItem, missionItemParent);
item->setSequenceNumber(seqNum++);
items.append(item);
}
}
void SurveyComplexItem::rotateEntryPoint(void)
{
if (_entryPoint == EntryLocationLast) {
......
src/MissionManager/SurveyComplexItem.h
View file @ 842ba9eb
......@@ -47,7 +47,6 @@ public:
// Overrides from TransectStyleComplexItem
void save (QJsonArray& planItems) final;
bool specifiesCoordinate (void) const final { return true; }
void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final;
void applyNewAltitude (double newAltitude) final;
double timeBetweenShots (void) final;
......@@ -109,8 +108,6 @@ private:
void _adjustTransectsToEntryPointLocation(QList<QList<QGeoCoordinate>>& transects);
bool _gridAngleIsNorthSouthTransects();
double _clampGridAngle90(double gridAngle);
void _buildAndAppendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent);
void _appendLoadedMissionItems (QList<MissionItem*>& items, QObject* missionItemParent);
bool _imagesEverywhere(void) const;
bool _triggerCamera(void) const;
bool _hasTurnaround(void) const;
......@@ -129,11 +126,6 @@ private:
// return true if vertex a can see vertex b
bool _VertexCanSeeOther(const QPolygonF& polygon, const QPointF* vertexA, const QPointF* vertexB);
bool _VertexIsReflex(const QPolygonF& polygon, const QPointF* vertex);
void _appendWaypoint(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, float holdTime, const QGeoCoordinate& coordinate);
void _appendSinglePhotoCapture(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum);
void _appendConditionGate(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate);
void _appendCameraTriggerDistance(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, float triggerDistance);
void _appendCameraTriggerDistanceUpdatePoint(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate, bool useConditionGate, float triggerDistance);
QMap<QString, FactMetaData*> _metaDataMap;
......@@ -172,7 +164,4 @@ private:
static const char* _jsonV3CameraOrientationLandscapeKey;
static const char* _jsonV3FixedValueIsAltitudeKey;
static const char* _jsonV3Refly90DegreesKey;
static const int _hoverAndCaptureDelaySeconds = 4;
};
src/MissionManager/SurveyComplexItemTest.cc
View file @ 842ba9eb
......@@ -23,7 +23,7 @@ SurveyComplexItemTest::SurveyComplexItemTest(void)
void SurveyComplexItemTest::init(void)
{
UnitTest::init();
TransectStyleComplexItemTestBase::init();
_rgSurveySignals[surveyVisualTransectPointsChangedIndex] = SIGNAL(visualTransectPointsChanged());
_rgSurveySignals[surveyCameraShotsChangedIndex] = SIGNAL(cameraShotsChanged());
......@@ -32,9 +32,6 @@ void SurveyComplexItemTest::init(void)
_rgSurveySignals[surveyRefly90DegreesChangedIndex] = SIGNAL(refly90DegreesChanged(bool));
_rgSurveySignals[surveyDirtyChangedIndex] = SIGNAL(dirtyChanged(bool));
_planViewSettings = qgcApp()->toolbox()->settingsManager()->planViewSettings();
_masterController = new PlanMasterController(this);
_controllerVehicle = _masterController->controllerVehicle();
_surveyItem = new SurveyComplexItem(_masterController, false /* flyView */, QString() /* kmlFile */, this /* parent */);
_mapPolygon = _surveyItem->surveyAreaPolygon();
_mapPolygon->appendVertices(_polyVertices);
......@@ -66,6 +63,8 @@ void SurveyComplexItemTest::cleanup(void)
{
delete _surveyItem;
delete _multiSpy;
TransectStyleComplexItemTestBase::cleanup();
}
void SurveyComplexItemTest::_testDirty(void)
......@@ -124,15 +123,8 @@ double SurveyComplexItemTest::_clampGridAngle180(double gridAngle)
return gridAngle;
}
void SurveyComplexItemTest::_setPolygon(void)
{
_mapPolygon->appendVertices(_polyVertices);
}
void SurveyComplexItemTest::_testGridAngle(void)
{
_setPolygon();
for (double gridAngle=-360.0; gridAngle<=360.0; gridAngle++) {
_surveyItem->gridAngle()->setRawValue(gridAngle);
......@@ -141,14 +133,14 @@ void SurveyComplexItemTest::_testGridAngle(void)
QGeoCoordinate firstTransectExit = gridPoints[1].value<QGeoCoordinate>();
double azimuth = firstTransectEntry.azimuthTo(firstTransectExit);
//qDebug() << gridAngle << azimuth << _clampGridAngle180(gridAngle) << _clampGridAngle180(azimuth);
QCOMPARE((int)_clampGridAngle180(gridAngle), (int)_clampGridAngle180(azimuth));
int clampGridAngle = qRound(_clampGridAngle180(gridAngle));
int clampAzimuth = qRound(_clampGridAngle180(azimuth));
QCOMPARE(clampGridAngle, clampAzimuth);
}
}
void SurveyComplexItemTest::_testEntryLocation(void)
{
_setPolygon();
for (double gridAngle=-360.0; gridAngle<=360.0; gridAngle++) {
_surveyItem->gridAngle()->setRawValue(gridAngle);
......@@ -165,40 +157,43 @@ void SurveyComplexItemTest::_testEntryLocation(void)
}
}
void SurveyComplexItemTest::_testItemCount(void)
{
QList<MissionItem*> items;
_setPolygon();
_surveyItem->hoverAndCapture()->setRawValue(false);
_surveyItem->cameraTriggerInTurnAround()->setRawValue(false);
_surveyItem->refly90Degrees()->setRawValue(false);
_surveyItem->appendMissionItems(items, this);
QCOMPARE(items.count() - 1, _surveyItem->lastSequenceNumber());
items.clear();
_surveyItem->hoverAndCapture()->setRawValue(false);
_surveyItem->cameraTriggerInTurnAround()->setRawValue(true);
_surveyItem->refly90Degrees()->setRawValue(false);
_surveyItem->appendMissionItems(items, this);
QCOMPARE(items.count() - 1, _surveyItem->lastSequenceNumber());
items.clear();
typedef struct {
bool hoverAndCapture;
bool triggerInTurnAround;
bool refly90;
bool hasTurnaround;
} TestCase_t;
_surveyItem->hoverAndCapture()->setRawValue(true);
_surveyItem->cameraTriggerInTurnAround()->setRawValue(false);
_surveyItem->refly90Degrees()->setRawValue(false);
_surveyItem->appendMissionItems(items, this);
QCOMPARE(items.count() - 1, _surveyItem->lastSequenceNumber());
items.clear();
QList<TestCase_t> rgTestCases;
for (int i=0; i<2; i++) {
for (int j=0; i<2; i++) {
for (int k=0; i<2; i++) {
for (int l=0; i<2; i++) {
TestCase_t testCase;
testCase.hoverAndCapture = i;
testCase.triggerInTurnAround = j;
testCase.refly90 = k;
testCase.hasTurnaround = l;
rgTestCases.append(testCase);
}
}
}
}
_surveyItem->hoverAndCapture()->setRawValue(true);
_surveyItem->cameraTriggerInTurnAround()->setRawValue(false);
_surveyItem->refly90Degrees()->setRawValue(true);
QList<MissionItem*> items;
for (const TestCase_t& testCase : rgTestCases) {
qDebug() << "hoverAndCapture:triggerInTurnAround:refly90:hasTuraround" << testCase.hoverAndCapture << testCase.triggerInTurnAround << testCase.refly90 << testCase.hasTurnaround;
_surveyItem->hoverAndCapture()->setRawValue(testCase.hoverAndCapture);
_surveyItem->cameraTriggerInTurnAround()->setRawValue(testCase.triggerInTurnAround);
_surveyItem->refly90Degrees()->setRawValue(testCase.refly90);
_surveyItem->turnAroundDistance()->setRawValue(testCase.hasTurnaround ? 50 : 0);
_surveyItem->appendMissionItems(items, this);
QCOMPARE(items.count() - 1, _surveyItem->lastSequenceNumber());
items.clear();
}
}
QList<MAV_CMD> SurveyComplexItemTest::_createExpectedCommands(bool hasTurnaround, bool useConditionGate)
......@@ -245,9 +240,7 @@ void SurveyComplexItemTest::_testItemGenerationWorker(bool imagesInTurnaround, b
_surveyItem->appendMissionItems(items, this);
#if 0
// Handy for debugging failures
for (const MissionItem* item : items) {
qDebug() << "Cmd" << item->command();
}
_printItemCommands(items);
#endif
QCOMPARE(items.count(), expectedCommands.count());
for (int i=0; i<expectedCommands.count(); i++) {
......@@ -283,7 +276,7 @@ void SurveyComplexItemTest::_testItemGeneration(void)
// Test cameraTriggerInTurnAround = true cases
QList<MAV_CMD> imagesInTurnaroundCommands = {
QList<MAV_CMD> imagesInTurnaroundWithTurnaroundDistanceCommands = {
// Transect 1
MAV_CMD_CONDITION_GATE, // First turaround
MAV_CMD_DO_SET_CAM_TRIGG_DIST,
......@@ -300,13 +293,33 @@ void SurveyComplexItemTest::_testItemGeneration(void)
MAV_CMD_DO_SET_CAM_TRIGG_DIST,
};
_testItemGenerationWorker(true /* imagesInTurnaround */, true /* hasTurnaround */, true /* useConditionGate */, imagesInTurnaroundCommands);
_testItemGenerationWorker(true /* imagesInTurnaround */, true /* hasTurnaround */, true /* useConditionGate */, imagesInTurnaroundWithTurnaroundDistanceCommands);
// Switch to non CONDITION_GATE usage
for (MAV_CMD& cmd : imagesInTurnaroundWithTurnaroundDistanceCommands) {
cmd = cmd == MAV_CMD_CONDITION_GATE ? MAV_CMD_NAV_WAYPOINT : cmd;
}
_testItemGenerationWorker(true /* imagesInTurnaround */, true /* hasTurnaround */, false /* useConditionGate */, imagesInTurnaroundWithTurnaroundDistanceCommands);
QList<MAV_CMD> imagesInTurnaroundWithoutTurnaroundDistanceCommands = {
// Transect 1
MAV_CMD_CONDITION_GATE, // Survey entry
MAV_CMD_DO_SET_CAM_TRIGG_DIST, // Camera trigger start for entire survey
MAV_CMD_NAV_WAYPOINT, // Survey exit
// Transect 2
MAV_CMD_CONDITION_GATE, // Survey entry
MAV_CMD_DO_SET_CAM_TRIGG_DIST, // Survey entry also has trigger start
MAV_CMD_CONDITION_GATE, // Survey exit
MAV_CMD_DO_SET_CAM_TRIGG_DIST, // Camera trigger stop for entire survey
};
_testItemGenerationWorker(true /* imagesInTurnaround */, false /* hasTurnaround */, true /* useConditionGate */, imagesInTurnaroundWithoutTurnaroundDistanceCommands);
// Switch to non CONDITION_GATE usage
for (MAV_CMD& cmd : imagesInTurnaroundCommands) {
for (MAV_CMD& cmd : imagesInTurnaroundWithoutTurnaroundDistanceCommands) {
cmd = cmd == MAV_CMD_CONDITION_GATE ? MAV_CMD_NAV_WAYPOINT : cmd;
}
_testItemGenerationWorker(true /* imagesInTurnaround */, true /* hasTurnaround */, false /* useConditionGate */, imagesInTurnaroundCommands);
_testItemGenerationWorker(true /* imagesInTurnaround */, false /* hasTurnaround */, false /* useConditionGate */, imagesInTurnaroundWithoutTurnaroundDistanceCommands);
}
void SurveyComplexItemTest::_testHoverCaptureItemGeneration(void)
......@@ -334,7 +347,6 @@ void SurveyComplexItemTest::_testHoverCaptureItemGeneration(void)
double triggerDistance = (polyHeightDistance / 3.0) + 1.0;
_surveyItem->cameraCalc()->adjustedFootprintFrontal()->setRawValue(triggerDistance);
qDebug() << "_testHoverCaptureItemGeneration";
_surveyItem->hoverAndCapture()->setRawValue(true);
_testItemGenerationWorker(false /* imagesInTurnaround */, true /* hasTurnaround */, true /* useConditionGate */, expectedCommands);
_testItemGenerationWorker(false /* imagesInTurnaround */, true /* hasTurnaround */, false /* useConditionGate */, expectedCommands);
......
src/MissionManager/SurveyComplexItemTest.h
View file @ 842ba9eb
......@@ -9,8 +9,7 @@
#pragma once
#include "UnitTest.h"
#include "TCPLink.h"
#include "TransectStyleComplexItemTestBase.h"
#include "MultiSignalSpy.h"
#include "SurveyComplexItem.h"
#include "PlanMasterController.h"
......@@ -19,7 +18,7 @@
#include <QGeoCoordinate>
/// Unit test for SurveyComplexItem
class SurveyComplexItemTest : public UnitTest
class SurveyComplexItemTest : public TransectStyleComplexItemTestBase
{
Q_OBJECT
......@@ -30,17 +29,28 @@ protected:
void init(void) final;
void cleanup(void) final;
#if 1
private slots:
void _testDirty(void);
void _testGridAngle(void);
void _testEntryLocation(void);
void _testItemGeneration(void);
void _testItemCount(void);
void _testHoverCaptureItemGeneration(void);
#else
// Handy mechanism to to a single test
private slots:
void _testItemCount(void);
private:
void _testDirty(void);
void _testGridAngle(void);
void _testEntryLocation(void);
void _testItemCount(void);
void _testItemGeneration(void);
void _testHoverCaptureItemGeneration(void);
#endif
private:
double _clampGridAngle180(double gridAngle);
void _setPolygon(void);
QList<MAV_CMD> _createExpectedCommands(bool hasTurnaround, bool useConditionGate);
void _testItemGenerationWorker(bool imagesInTurnaround, bool hasTurnaround, bool useConditionGate, const QList<MAV_CMD>& expectedCommands);
......@@ -68,12 +78,9 @@ private:
static const size_t _cSurveySignals = surveyMaxSignalIndex;
const char* _rgSurveySignals[_cSurveySignals];
PlanMasterController* _masterController = nullptr;
Vehicle* _controllerVehicle = nullptr;
MultiSignalSpy* _multiSpy = nullptr;
SurveyComplexItem* _surveyItem = nullptr;
QGCMapPolygon* _mapPolygon = nullptr;
PlanViewSettings* _planViewSettings = nullptr;
QList<QGeoCoordinate> _polyVertices;
static const int _expectedTransectCount = 2;
......
src/MissionManager/TransectStyleComplexItem.cc
View file @ 842ba9eb
......@@ -37,8 +37,6 @@ const char* TransectStyleComplexItem::_jsonItemsKey = "Ite
const char* TransectStyleComplexItem::_jsonFollowTerrainKey = "FollowTerrain";
const char* TransectStyleComplexItem::_jsonCameraShotsKey = "CameraShots";
const int TransectStyleComplexItem::_terrainQueryTimeoutMsecs = 1000;
TransectStyleComplexItem::TransectStyleComplexItem(PlanMasterController* masterController, bool flyView, QString settingsGroup, QObject* parent)
: ComplexMissionItem (masterController, flyView, parent)
, _sequenceNumber (0)
......@@ -337,10 +335,10 @@ double TransectStyleComplexItem::coveredArea(void) const
bool TransectStyleComplexItem::_hasTurnaround(void) const
{
return _turnaroundDistance() > 0;
return _turnAroundDistance() > 0;
}
double TransectStyleComplexItem::_turnaroundDistance(void) const
double TransectStyleComplexItem::_turnAroundDistance(void) const
{
return _turnAroundDistanceFact.rawValue().toDouble();
}
......@@ -755,16 +753,26 @@ int TransectStyleComplexItem::lastSequenceNumber(void) const
int itemCount = 0;
for (const QList<CoordInfo_t>& rgCoordInfo: _transects) {
itemCount += rgCoordInfo.count() * (hoverAndCaptureEnabled() ? 2 : 1);
int commandsPerCoord = 1; // Waypoint command
if (hoverAndCaptureEnabled()) {
commandsPerCoord++; // Camera trigger
}
itemCount += rgCoordInfo.count() * commandsPerCoord;
if (hoverAndCaptureEnabled() && _turnAroundDistance() != 0) {
// The turnaround points do not have camera triggers on them
itemCount -= 2;
}
}
if (!hoverAndCaptureEnabled() && triggerCamera()) {
if (_cameraTriggerInTurnAroundFact.rawValue().toBool()) {
// One camera start/stop for beginning/end of entire survey
itemCount += 2;
// One camera start for each transect
itemCount += _transects.count();
itemCount += _transects.count(); // One camera start for each transect entry
itemCount++; // Single camera stop and the very end
if (_turnAroundDistance() != 0) {
// If there are turnarounds then there is an additional camera start on the first turnaround
itemCount++;
}
} else {
// Each transect will have a camera start and stop in it
itemCount += _transects.count() * 2;
......@@ -797,7 +805,184 @@ void TransectStyleComplexItem::_followTerrainChanged(bool followTerrain)
void TransectStyleComplexItem::_handleHoverAndCaptureEnabled(QVariant enabled)
{
if (enabled.toBool() && _cameraTriggerInTurnAroundFact.rawValue().toBool()) {
qDebug() << "_handleHoverAndCaptureEnabled";
_cameraTriggerInTurnAroundFact.setRawValue(false);
}
}
void TransectStyleComplexItem::appendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
if (_loadedMissionItems.count()) {
// We have mission items from the loaded plan, use those
_appendLoadedMissionItems(items, missionItemParent);
} else {
// Build the mission items on the fly
_buildAndAppendMissionItems(items, missionItemParent);
}
}
void TransectStyleComplexItem::_appendWaypoint(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, float holdTime, const QGeoCoordinate& coordinate)
{
MissionItem* item = new MissionItem(seqNum++,
MAV_CMD_NAV_WAYPOINT,
mavFrame,
holdTime,
0.0, // No acceptance radius specified
0.0, // Pass through waypoint
std::numeric_limits<double>::quiet_NaN(), // Yaw unchanged
coordinate.latitude(),
coordinate.longitude(),
coordinate.altitude(),
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
}
void TransectStyleComplexItem::_appendSinglePhotoCapture(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum)
{
MissionItem* item = new MissionItem(seqNum++,
MAV_CMD_IMAGE_START_CAPTURE,
MAV_FRAME_MISSION,
0, // Reserved (Set to 0)
0, // Interval (none)
1, // Take 1 photo
qQNaN(), qQNaN(), qQNaN(), qQNaN(), // param 4-7 reserved
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
}
void TransectStyleComplexItem::_appendConditionGate(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate)
{
MissionItem* item = new MissionItem(seqNum++,
MAV_CMD_CONDITION_GATE,
mavFrame,
0, // Gate is orthogonal to path
0, // Ignore altitude
0, 0, // Param 3-4 ignored
coordinate.latitude(),
coordinate.longitude(),
0, // No altitude
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
}
void TransectStyleComplexItem::_appendCameraTriggerDistance(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, float triggerDistance)
{
MissionItem* item = new MissionItem(seqNum++,
MAV_CMD_DO_SET_CAM_TRIGG_DIST,
MAV_FRAME_MISSION,
triggerDistance,
0, // shutter integration (ignore)
1, // 1 - trigger one image immediately, both and entry and exit to get full coverage
0, 0, 0, 0, // param 4-7 unused
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
}
void TransectStyleComplexItem::_appendCameraTriggerDistanceUpdatePoint(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate, bool useConditionGate, float triggerDistance)
{
if (useConditionGate) {
_appendConditionGate(items, missionItemParent, seqNum, mavFrame, coordinate);
} else {
_appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, coordinate);
}
_appendCameraTriggerDistance(items, missionItemParent, seqNum, triggerDistance);
}
void TransectStyleComplexItem::_buildAndAppendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
qCDebug(TransectStyleComplexItemLog) << "_buildAndAppendMissionItems";
// Now build the mission items from the transect points
int seqNum = _sequenceNumber;
bool imagesInTurnaround = _cameraTriggerInTurnAroundFact.rawValue().toBool();
bool hasTurnarounds = _turnAroundDistance() != 0;
bool addTriggerAtBeginningEnd = !hoverAndCaptureEnabled() && imagesInTurnaround && triggerCamera();
bool useConditionGate = _controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(MAV_CMD_CONDITION_GATE) &&
triggerCamera() &&
!hoverAndCaptureEnabled();
MAV_FRAME mavFrame = followTerrain() || !_cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT;
// Note: The code below is written to be understable as oppose to being compact and/or remove duplicate code
int transectIndex = 0;
for (const QList<TransectStyleComplexItem::CoordInfo_t>& transect: _transects) {
bool entryTurnaround = true;
for (const CoordInfo_t& transectCoordInfo: transect) {
switch (transectCoordInfo.coordType) {
case CoordTypeTurnaround:
{
bool firstEntryTurnaround = transectIndex == 0 && entryTurnaround;
bool lastExitTurnaround = transectIndex == _transects.count() - 1 && !entryTurnaround;
if (addTriggerAtBeginningEnd && (firstEntryTurnaround || lastExitTurnaround)) {
_appendCameraTriggerDistanceUpdatePoint(items, missionItemParent, seqNum, mavFrame, transectCoordInfo.coord, useConditionGate, firstEntryTurnaround ? triggerDistance() : 0);
} else {
_appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord);
}
entryTurnaround = false;
}
break;
case CoordTypeInterior:
case CoordTypeInteriorTerrainAdded:
_appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord);
break;
case CoordTypeInteriorHoverTrigger:
_appendWaypoint(items, missionItemParent, seqNum, mavFrame, _hoverAndCaptureDelaySeconds, transectCoordInfo.coord);
_appendSinglePhotoCapture(items, missionItemParent, seqNum);
break;
case CoordTypeSurveyEntry:
if (triggerCamera()) {
if (hoverAndCaptureEnabled()) {
_appendWaypoint(items, missionItemParent, seqNum, mavFrame, _hoverAndCaptureDelaySeconds, transectCoordInfo.coord);
_appendSinglePhotoCapture(items, missionItemParent, seqNum);
} else {
// We always add a trigger start to survey entry. Even for imagesInTurnaround = true. This allows you to resume a mission and refly a transect
_appendCameraTriggerDistanceUpdatePoint(items, missionItemParent, seqNum, mavFrame, transectCoordInfo.coord, useConditionGate, triggerDistance());
}
} else {
_appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord);
}
break;
case CoordTypeSurveyExit:
bool lastSurveyExit = transectIndex == _transects.count() - 1;
if (triggerCamera()) {
if (hoverAndCaptureEnabled()) {
_appendWaypoint(items, missionItemParent, seqNum, mavFrame, _hoverAndCaptureDelaySeconds, transectCoordInfo.coord);
_appendSinglePhotoCapture(items, missionItemParent, seqNum);
} else if (addTriggerAtBeginningEnd && !hasTurnarounds && lastSurveyExit) {
_appendCameraTriggerDistanceUpdatePoint(items, missionItemParent, seqNum, mavFrame, transectCoordInfo.coord, useConditionGate, 0 /* triggerDistance */);
} else if (imagesInTurnaround) {
_appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord);
} else {
// If we get this far it means the camera is triggering start/stop for each transect
_appendCameraTriggerDistanceUpdatePoint(items, missionItemParent, seqNum, mavFrame, transectCoordInfo.coord, useConditionGate, 0 /* triggerDistance */);
}
} else {
_appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord);
}
break;
}
}
transectIndex++;
}
}
void TransectStyleComplexItem::_appendLoadedMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
qCDebug(TransectStyleComplexItemLog) << "_appendLoadedMissionItems";
int seqNum = _sequenceNumber;
for (const MissionItem* loadedMissionItem: _loadedMissionItems) {
MissionItem* item = new MissionItem(*loadedMissionItem, missionItemParent);
item->setSequenceNumber(seqNum++);
items.append(item);
}
}
src/MissionManager/TransectStyleComplexItem.h
View file @ 842ba9eb
......@@ -75,7 +75,7 @@ public:
bool triggerCamera (void) const { return triggerDistance() != 0; }
// Used internally only by unit tests
int _transectCount(void) { return _transects.count(); }
int _transectCount(void) const { return _transects.count(); }
// Overrides from ComplexMissionItem
......@@ -90,7 +90,7 @@ public:
void save (QJsonArray& planItems) override = 0;
bool specifiesCoordinate (void) const override = 0;
void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) override = 0;
void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final;
void applyNewAltitude (double newAltitude) override = 0;
bool dirty (void) const final { return _dirty; }
......@@ -151,7 +151,14 @@ protected:
void _setCameraShots (int cameraShots);
double _triggerDistance (void) const;
bool _hasTurnaround (void) const;
double _turnaroundDistance (void) const;
double _turnAroundDistance (void) const;
void _appendWaypoint (QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, float holdTime, const QGeoCoordinate& coordinate);
void _appendSinglePhotoCapture (QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum);
void _appendConditionGate (QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate);
void _appendCameraTriggerDistance (QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, float triggerDistance);
void _appendCameraTriggerDistanceUpdatePoint(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate, bool useConditionGate, float triggerDistance);
void _buildAndAppendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent);
void _appendLoadedMissionItems (QList<MissionItem*>& items, QObject* missionItemParent);
int _sequenceNumber;
QGeoCoordinate _coordinate;
......@@ -206,7 +213,8 @@ protected:
static const char* _jsonFollowTerrainKey;
static const char* _jsonCameraShotsKey;
static const int _terrainQueryTimeoutMsecs;
static const int _terrainQueryTimeoutMsecs= 1000;
static const int _hoverAndCaptureDelaySeconds = 4;
private slots:
void _reallyQueryTransectsPathHeightInfo(void);
......
src/MissionManager/TransectStyleComplexItemTest.cc
View file @ 842ba9eb
......@@ -20,10 +20,8 @@ TransectStyleComplexItemTest::TransectStyleComplexItemTest(void)
void TransectStyleComplexItemTest::init(void)
{
UnitTest::init();
TransectStyleComplexItemTestBase::init();
_masterController = new PlanMasterController(this);
_controllerVehicle = _masterController->controllerVehicle();
_transectStyleItem = new TransectStyleItem(_masterController, this);
_transectStyleItem->cameraTriggerInTurnAround()->setRawValue(false);
_transectStyleItem->cameraCalc()->cameraName()->setRawValue(_transectStyleItem->cameraCalc()->customCameraName());
......@@ -50,6 +48,7 @@ void TransectStyleComplexItemTest::cleanup(void)
{
delete _transectStyleItem;
delete _multiSpy;
TransectStyleComplexItemTestBase::cleanup();
}
void TransectStyleComplexItemTest::_testDirty(void)
......
src/MissionManager/TransectStyleComplexItemTest.h
View file @ 842ba9eb
......@@ -9,7 +9,7 @@
#pragma once
#include "UnitTest.h"
#include "TransectStyleComplexItemTestBase.h"
#include "MultiSignalSpy.h"
#include "CorridorScanComplexItem.h"
#include "PlanMasterController.h"
......@@ -18,7 +18,7 @@
class TransectStyleItem;
class TransectStyleComplexItemTest : public UnitTest
class TransectStyleComplexItemTest : public TransectStyleComplexItemTestBase
{
Q_OBJECT
......@@ -73,8 +73,6 @@ private:
static const size_t _cSignals = maxSignalIndex;
const char* _rgSignals[_cSignals];
PlanMasterController* _masterController = nullptr;
Vehicle* _controllerVehicle = nullptr;
MultiSignalSpy* _multiSpy = nullptr;
QList<QGeoCoordinate> _polygonVertices;
TransectStyleItem* _transectStyleItem = nullptr;
......@@ -94,7 +92,6 @@ public:
// Overrides from VisualMissionItem
void save (QJsonArray& missionItems) final { Q_UNUSED(missionItems); }
bool specifiesCoordinate (void) const final { return true; }
void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final { Q_UNUSED(items); Q_UNUSED(missionItemParent); }
void applyNewAltitude (double newAltitude) final { Q_UNUSED(newAltitude); }
double additionalTimeDelay (void) const final { return 0; }
......
src/MissionManager/TransectStyleComplexItemTestBase.cc 0 → 100644
View file @ 842ba9eb
/****************************************************************************
*
* (c) 2009-2020 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 "TransectStyleComplexItemTestBase.h"
#include "QGCApplication.h"
TransectStyleComplexItemTestBase::TransectStyleComplexItemTestBase(void)
{
}
void TransectStyleComplexItemTestBase::init(void)
{
UnitTest::init();
_planViewSettings = qgcApp()->toolbox()->settingsManager()->planViewSettings();
_masterController = new PlanMasterController(this);
_controllerVehicle = _masterController->controllerVehicle();
}
void TransectStyleComplexItemTestBase::cleanup(void)
{
delete _masterController;
_planViewSettings = nullptr;
_masterController = nullptr;
_controllerVehicle = nullptr;
UnitTest::cleanup();
}
void TransectStyleComplexItemTestBase::_printItemCommands(QList<MissionItem*> items)
{
// Handy for debugging failures
for (int i=0; i<items.count(); i++) {
MissionItem* item = items[i];
qDebug() << "Index:Cmd" << i << item->command();
}
}
src/MissionManager/TransectStyleComplexItemTestBase.h 0 → 100644
View file @ 842ba9eb
/****************************************************************************
*
* (c) 2009-2020 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 "CorridorScanComplexItem.h"
#include "PlanMasterController.h"
#include "PlanViewSettings.h"
/// Base class for all TransectStyleComplexItem unit tests
class TransectStyleComplexItemTestBase : public UnitTest
{
Q_OBJECT
public:
TransectStyleComplexItemTestBase(void);
protected:
void init (void) override;
void cleanup(void) override;
void _printItemCommands(QList<MissionItem*> items);
PlanMasterController* _masterController = nullptr;
Vehicle* _controllerVehicle = nullptr;
PlanViewSettings* _planViewSettings = nullptr;
};
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment