zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #6237 from DonLakeFlyer/CorridorTerrain 

Corridor Scan Terrain: Add support for max rates and tolerance
QGC4.4
Don Gagne 7 years ago committed by GitHub
parent
32b4b1da9c 7973c6b3e5
commit
5e70503264
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
14 changed files with 465 additions and 287 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 237
      src/MissionManager/CorridorScanComplexItem.cc
  2. 5
      src/MissionManager/CorridorScanComplexItem.h
  3. 53
      src/MissionManager/CorridorScanComplexItemTest.cc
  4. 1
      src/MissionManager/CorridorScanComplexItemTest.h
  5. 2
      src/MissionManager/MissionController.h
  6. 3
      src/MissionManager/SimpleMissionItem.cc
  7. 337
      src/MissionManager/TransectStyleComplexItem.cc
  8. 51
      src/MissionManager/TransectStyleComplexItem.h
  9. 2
      src/MissionManager/TransectStyleComplexItemTest.cc
  10. 23
      src/MissionManager/TransectStyleComplexItemTest.h
  11. 19
      src/MissionManager/VisualMissionItem.cc
  12. 2
      src/MissionManager/VisualMissionItem.h
  13. 2
      src/PlanView/CorridorScanMapVisual.qml
  14. 15
      src/Terrain/TerrainQuery.cc

237
src/MissionManager/CorridorScanComplexItem.cc
Unescape View File

@ -48,33 +48,9 @@ CorridorScanComplexItem::CorridorScanComplexItem(Vehicle* vehicle, QObject* pare @@ -48,33 +48,9 @@ CorridorScanComplexItem::CorridorScanComplexItem(Vehicle* vehicle, QObject* pare
connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this, &CorridorScanComplexItem::exitCoordinateHasRelativeAltitudeChanged);
connect(&_corridorPolyline, &QGCMapPolyline::dirtyChanged, this, &CorridorScanComplexItem::_polylineDirtyChanged);
connect(&_corridorPolyline, &QGCMapPolyline::countChanged, this, &CorridorScanComplexItem::_polylineCountChanged);
connect(&_corridorPolyline, &QGCMapPolyline::pathChanged, this, &CorridorScanComplexItem::_rebuildCorridor);
connect(&_corridorWidthFact, &Fact::valueChanged, this, &CorridorScanComplexItem::_rebuildCorridor);
_rebuildCorridor();
}
void CorridorScanComplexItem::_polylineCountChanged(int count)
{
Q_UNUSED(count);
emit lastSequenceNumberChanged(lastSequenceNumber());
}
int CorridorScanComplexItem::lastSequenceNumber(void) const
{
int itemCount = _transectPoints.count(); // Each transpect point represents a waypoint item
if (_cameraTriggerInTurnAroundFact.rawValue().toBool()) {
// Only one camera start and on camera stop
itemCount += 2;
} else {
// Each transect will have a camera start and stop in it
itemCount += _transectCount() * 2;
}
return _sequenceNumber + itemCount - 1;
connect(&_corridorPolyline, &QGCMapPolyline::pathChanged, this, &CorridorScanComplexItem::_rebuildCorridorPolygon);
connect(&_corridorWidthFact, &Fact::valueChanged, this, &CorridorScanComplexItem::_rebuildCorridorPolygon);
}
void CorridorScanComplexItem::save(QJsonArray& planItems)
@ -147,7 +123,7 @@ bool CorridorScanComplexItem::load(const QJsonObject& complexObject, int sequenc @@ -147,7 +123,7 @@ bool CorridorScanComplexItem::load(const QJsonObject& complexObject, int sequenc
_entryPoint = complexObject[_jsonEntryPointKey].toInt();
_rebuildCorridor();
_rebuildTransects();
_ignoreRecalc = false;
@ -183,58 +159,42 @@ void CorridorScanComplexItem::_buildAndAppendMissionItems(QList<MissionItem*>& i @@ -183,58 +159,42 @@ void CorridorScanComplexItem::_buildAndAppendMissionItems(QList<MissionItem*>& i
{
qCDebug(CorridorScanComplexItemLog) << "_buildAndAppendMissionItems";
// First adjust for terrain (this will set altitudes into _transectionPoints in all cases
_adjustTransectPointsForTerrain();
// Now build the mission items from the transect points
MissionItem* item;
int seqNum = _sequenceNumber;
bool imagesEverywhere = _cameraTriggerInTurnAroundFact.rawValue().toBool();
bool addTriggerAtBeginning = imagesEverywhere;
bool firstPoint = true;
bool entryPoint = true;
bool firstOverallPoint = true;
MAV_FRAME mavFrame = followTerrain() || !_cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT;
foreach (const QVariant& transectPointVar, _transectPoints) {
QGeoCoordinate transectPoint = transectPointVar.value<QGeoCoordinate>();
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
transectPoint.latitude(),
transectPoint.longitude(),
qAbs(transectPoint.altitude()), // qAbs since negative value indicates survey edge
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
//qDebug() << "_buildAndAppendMissionItems";
foreach (const QList<TransectStyleComplexItem::CoordInfo_t>& transect, _transects) {
bool entryPoint = true;
//qDebug() << "start transect";
foreach (const CoordInfo_t& transectCoordInfo, transect) {
//qDebug() << transectCoordInfo.coordType;
if (firstPoint && 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
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);
}
firstPoint = false;
if (transectPoint.altitude() < 0 && !imagesEverywhere) {
if (entryPoint) {
if (firstOverallPoint && addTriggerAtBeginning) {
// Start triggering
addTriggerAtBeginning = false;
item = new MissionItem(seqNum++,
MAV_CMD_DO_SET_CAM_TRIGG_DIST,
MAV_FRAME_MISSION,
@ -246,21 +206,39 @@ void CorridorScanComplexItem::_buildAndAppendMissionItems(QList<MissionItem*>& i @@ -246,21 +206,39 @@ void CorridorScanComplexItem::_buildAndAppendMissionItems(QList<MissionItem*>& i
false, // isCurrentItem
missionItemParent);
items.append(item);
} else {
// 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);
}
entryPoint = !entryPoint;
firstOverallPoint = false;
if (transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge && !imagesEverywhere) {
if (entryPoint) {
// Start of transect, start triggering
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);
} else {
// 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);
}
entryPoint = !entryPoint;
}
}
}
@ -312,7 +290,7 @@ void CorridorScanComplexItem::rotateEntryPoint(void) @@ -312,7 +290,7 @@ void CorridorScanComplexItem::rotateEntryPoint(void)
_entryPoint = 0;
}
_rebuildCorridor();
_rebuildTransects();
}
void CorridorScanComplexItem::_rebuildCorridorPolygon(void)
@ -349,7 +327,7 @@ void CorridorScanComplexItem::_rebuildTransectsPhase1(void) @@ -349,7 +327,7 @@ void CorridorScanComplexItem::_rebuildTransectsPhase1(void)
_loadedMissionItemsParent = NULL;
}
_transectPoints.clear();
_transects.clear();
_transectsPathHeightInfo.clear();
double transectSpacing = _cameraCalc.adjustedFootprintSide()->rawValue().toDouble();
@ -360,8 +338,9 @@ void CorridorScanComplexItem::_rebuildTransectsPhase1(void) @@ -360,8 +338,9 @@ void CorridorScanComplexItem::_rebuildTransectsPhase1(void)
if (_corridorPolyline.count() >= 2) {
// First build up the transects all going the same direction
QList<QList<QGeoCoordinate>> transects;
//qDebug() << "_rebuildTransectsPhase1";
for (int i=0; i<transectCount; i++) {
//qDebug() << "start transect";
double offsetDistance;
if (transectCount == 1) {
// Single transect is flown over scan line
@ -371,24 +350,44 @@ void CorridorScanComplexItem::_rebuildTransectsPhase1(void) @@ -371,24 +350,44 @@ void CorridorScanComplexItem::_rebuildTransectsPhase1(void)
offsetDistance = halfWidth - normalizedTransectPosition;
}
QList<QGeoCoordinate> transect = _corridorPolyline.offsetPolyline(offsetDistance);
transect[0].setAltitude(_surveyEdgeIndicator);
transect[1].setAltitude(_surveyEdgeIndicator);
// Turn transect into CoordInfo transect
QList<TransectStyleComplexItem::CoordInfo_t> transect;
QList<QGeoCoordinate> transectCoords = _corridorPolyline.offsetPolyline(offsetDistance);
for (int j=1; j<transectCoords.count() - 1; j++) {
TransectStyleComplexItem::CoordInfo_t coordInfo = { transectCoords[j], CoordTypeInterior };
transect.append(coordInfo);
}
TransectStyleComplexItem::CoordInfo_t coordInfo = { transectCoords.first(), CoordTypeSurveyEdge };
transect.prepend(coordInfo);
coordInfo = { transectCoords.last(), CoordTypeSurveyEdge };
transect.append(coordInfo);
// Extend the transect ends for turnaround
if (_hasTurnaround()) {
QGeoCoordinate extensionCoord;
// Extend the transect ends for turnaround
double azimuth = transect[0].azimuthTo(transect[1]);
extensionCoord = transect[0].atDistanceAndAzimuth(-_turnAroundDistanceFact.rawValue().toDouble(), azimuth);
extensionCoord.setAltitude(qQNaN());
transect.prepend(extensionCoord);
azimuth = transect.last().azimuthTo(transect[transect.count() - 2]);
extensionCoord = transect.last().atDistanceAndAzimuth(-_turnAroundDistanceFact.rawValue().toDouble(), azimuth);
extensionCoord.setAltitude(qQNaN());
transect.append(extensionCoord);
QGeoCoordinate turnaroundCoord;
double turnAroundDistance = _turnAroundDistanceFact.rawValue().toDouble();
double azimuth = transectCoords[0].azimuthTo(transectCoords[1]);
turnaroundCoord = transectCoords[0].atDistanceAndAzimuth(-turnAroundDistance, azimuth);
turnaroundCoord.setAltitude(qQNaN());
TransectStyleComplexItem::CoordInfo_t coordInfo = { turnaroundCoord, CoordTypeTurnaround };
transect.prepend(coordInfo);
azimuth = transectCoords.last().azimuthTo(transectCoords[transectCoords.count() - 2]);
turnaroundCoord = transectCoords.last().atDistanceAndAzimuth(-turnAroundDistance, azimuth);
turnaroundCoord.setAltitude(qQNaN());
coordInfo = { turnaroundCoord, CoordTypeTurnaround };
transect.append(coordInfo);
}
#if 0
qDebug() << "transect debug";
foreach (const TransectStyleComplexItem::CoordInfo_t& coordInfo, transect) {
qDebug() << coordInfo.coordType;
}
#endif
transects.append(transect);
_transects.append(transect);
normalizedTransectPosition += transectSpacing;
}
@ -419,30 +418,30 @@ void CorridorScanComplexItem::_rebuildTransectsPhase1(void) @@ -419,30 +418,30 @@ void CorridorScanComplexItem::_rebuildTransectsPhase1(void)
break;
}
if (reverseTransects) {
QList<QList<QGeoCoordinate>> reversedTransects;
foreach (const QList<QGeoCoordinate>& transect, transects) {
QList<QList<TransectStyleComplexItem::CoordInfo_t>> reversedTransects;
foreach (const QList<TransectStyleComplexItem::CoordInfo_t>& transect, _transects) {
reversedTransects.prepend(transect);
}
transects = reversedTransects;
_transects = reversedTransects;
}
if (reverseVertices) {
for (int i=0; i<transects.count(); i++) {
QList<QGeoCoordinate> reversedVertices;
foreach (const QGeoCoordinate& vertex, transects[i]) {
for (int i=0; i<_transects.count(); i++) {
QList<TransectStyleComplexItem::CoordInfo_t> reversedVertices;
foreach (const TransectStyleComplexItem::CoordInfo_t& vertex, _transects[i]) {
reversedVertices.prepend(vertex);
}
transects[i] = reversedVertices;
_transects[i] = reversedVertices;
}
}
// Convert the list of transects to grid points
// Adjust to lawnmower pattern
reverseVertices = false;
for (int i=0; i<transects.count(); i++) {
for (int i=0; i<_transects.count(); i++) {
// We must reverse the vertices for every other transect in order to make a lawnmower pattern
QList<QGeoCoordinate> transectVertices = transects[i];
QList<TransectStyleComplexItem::CoordInfo_t> transectVertices = _transects[i];
if (reverseVertices) {
reverseVertices = false;
QList<QGeoCoordinate> reversedVertices;
QList<TransectStyleComplexItem::CoordInfo_t> reversedVertices;
for (int j=transectVertices.count()-1; j>=0; j--) {
reversedVertices.append(transectVertices[j]);
}
@ -450,23 +449,17 @@ void CorridorScanComplexItem::_rebuildTransectsPhase1(void) @@ -450,23 +449,17 @@ void CorridorScanComplexItem::_rebuildTransectsPhase1(void)
} else {
reverseVertices = true;
}
for (int i=0; i<transectVertices.count(); i++) {
_transectPoints.append(QVariant::fromValue((transectVertices[i])));
}
normalizedTransectPosition += transectSpacing;
_transects[i] = transectVertices;
}
}
_queryTransectsPathHeightInfo();
}
void CorridorScanComplexItem::_rebuildTransectsPhase2(void)
{
// Calculate distance flown for complex item
_complexDistance = 0;
for (int i=0; i<_transectPoints.count() - 2; i++) {
_complexDistance += _transectPoints[i].value<QGeoCoordinate>().distanceTo(_transectPoints[i+1].value<QGeoCoordinate>());
for (int i=0; i<_visualTransectPoints.count() - 2; i++) {
_complexDistance += _visualTransectPoints[i].value<QGeoCoordinate>().distanceTo(_visualTransectPoints[i+1].value<QGeoCoordinate>());
}
if (_cameraTriggerInTurnAroundFact.rawValue().toBool()) {
@ -476,23 +469,15 @@ void CorridorScanComplexItem::_rebuildTransectsPhase2(void) @@ -476,23 +469,15 @@ void CorridorScanComplexItem::_rebuildTransectsPhase2(void)
_cameraShots = singleTransectImageCount * _transectCount();
}
_coordinate = _transectPoints.count() ? _transectPoints.first().value<QGeoCoordinate>() : QGeoCoordinate();
_exitCoordinate = _transectPoints.count() ? _transectPoints.last().value<QGeoCoordinate>() : QGeoCoordinate();
_coordinate = _visualTransectPoints.count() ? _visualTransectPoints.first().value<QGeoCoordinate>() : QGeoCoordinate();
_exitCoordinate = _visualTransectPoints.count() ? _visualTransectPoints.last().value<QGeoCoordinate>() : QGeoCoordinate();
emit transectPointsChanged();
emit cameraShotsChanged();
emit complexDistanceChanged();
emit coordinateChanged(_coordinate);
emit exitCoordinateChanged(_exitCoordinate);
}
void CorridorScanComplexItem::_rebuildCorridor(void)
{
_rebuildCorridorPolygon();
_rebuildTransectsPhase1();
_rebuildTransectsPhase2();
}
bool CorridorScanComplexItem::readyForSave(void) const
{
return TransectStyleComplexItem::readyForSave();

5
src/MissionManager/CorridorScanComplexItem.h
Unescape View File

@ -36,7 +36,6 @@ public: @@ -36,7 +36,6 @@ public:
Q_INVOKABLE void rotateEntryPoint(void);
// Overrides from ComplexMissionItem
int lastSequenceNumber (void) const final;
bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final;
QString mapVisualQML (void) const final { return QStringLiteral("CorridorScanMapVisual.qml"); }
@ -56,8 +55,7 @@ public: @@ -56,8 +55,7 @@ public:
private slots:
void _polylineDirtyChanged (bool dirty);
void _polylineCountChanged (int count);
void _rebuildCorridor (void);
void _rebuildCorridorPolygon (void);
// Overrides from TransectStyleComplexItem
void _rebuildTransectsPhase1 (void) final;
@ -65,7 +63,6 @@ private slots: @@ -65,7 +63,6 @@ private slots:
private:
int _transectCount (void) const;
void _rebuildCorridorPolygon (void);
void _buildAndAppendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent);
void _appendLoadedMissionItems (QList<MissionItem*>& items, QObject* missionItemParent);

53
src/MissionManager/CorridorScanComplexItemTest.cc
Unescape View File

@ -24,7 +24,12 @@ void CorridorScanComplexItemTest::init(void) @@ -24,7 +24,12 @@ void CorridorScanComplexItemTest::init(void)
_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
// vehicleSpeed need for terrain calcs
MissionController::MissionFlightStatus_t missionFlightStatus;
missionFlightStatus.vehicleSpeed = 5;
_corridorItem->setMissionFlightStatus(missionFlightStatus);
_setPolyline();
_corridorItem->setDirty(false);
@ -130,10 +135,27 @@ void CorridorScanComplexItemTest::_testEntryLocation(void) @@ -130,10 +135,27 @@ void CorridorScanComplexItemTest::_testEntryLocation(void)
}
#endif
void CorridorScanComplexItemTest::_waitForReadyForSave(void)
{
int loops = 0;
while (loops++ < 8) {
if (_corridorItem->readyForSave()) {
return;
}
QTest::qWait(500);
}
QVERIFY(false);
}
void CorridorScanComplexItemTest::_testItemCount(void)
{
QList<MissionItem*> items;
_corridorItem->turnAroundDistance()->setRawValue(0);
_corridorItem->cameraTriggerInTurnAround()->setRawValue(true);
_corridorItem->appendMissionItems(items, this);
QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber());
items.clear();
_corridorItem->turnAroundDistance()->setRawValue(0);
_corridorItem->cameraTriggerInTurnAround()->setRawValue(false);
@ -141,7 +163,7 @@ void CorridorScanComplexItemTest::_testItemCount(void) @@ -141,7 +163,7 @@ void CorridorScanComplexItemTest::_testItemCount(void)
QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber());
items.clear();
_corridorItem->turnAroundDistance()->setRawValue(0);
_corridorItem->turnAroundDistance()->setRawValue(20);
_corridorItem->cameraTriggerInTurnAround()->setRawValue(true);
_corridorItem->appendMissionItems(items, this);
QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber());
@ -153,11 +175,38 @@ void CorridorScanComplexItemTest::_testItemCount(void) @@ -153,11 +175,38 @@ void CorridorScanComplexItemTest::_testItemCount(void)
QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber());
items.clear();
#if 0
// Terrain queries seem to take random amount of time so these don't work 100%
_corridorItem->setFollowTerrain(true);
_corridorItem->turnAroundDistance()->setRawValue(0);
_corridorItem->cameraTriggerInTurnAround()->setRawValue(true);
_waitForReadyForSave();
_corridorItem->appendMissionItems(items, this);
QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber());
items.clear();
_corridorItem->turnAroundDistance()->setRawValue(0);
_corridorItem->cameraTriggerInTurnAround()->setRawValue(false);
_waitForReadyForSave();
_corridorItem->appendMissionItems(items, this);
QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber());
items.clear();
_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();
_corridorItem->appendMissionItems(items, this);
QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber());
items.clear();
#endif
}
void CorridorScanComplexItemTest::_testPathChanges(void)

1
src/MissionManager/CorridorScanComplexItemTest.h
Unescape View File

@ -36,6 +36,7 @@ private slots: @@ -36,6 +36,7 @@ private slots:
private:
void _setPolyline(void);
void _waitForReadyForSave(void);
enum {
corridorPolygonPathChangedIndex = 0,

2
src/MissionManager/MissionController.h
Unescape View File

@ -41,7 +41,7 @@ public: @@ -41,7 +41,7 @@ public:
MissionController(PlanMasterController* masterController, QObject* parent = NULL);
~MissionController();
typedef struct {
typedef struct _MissionFlightStatus_t {
double maxTelemetryDistance;
double totalDistance;
double totalTime;

3
src/MissionManager/SimpleMissionItem.cc
Unescape View File

@ -125,6 +125,9 @@ SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, bool editMode, const Miss @@ -125,6 +125,9 @@ SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, bool editMode, const Miss
if (editMode) {
_rebuildFacts();
}
// Signal coordinate changed to kick off terrain query
emit coordinateChanged(coordinate());
}
SimpleMissionItem::SimpleMissionItem(const SimpleMissionItem& other, QObject* parent)

337
src/MissionManager/TransectStyleComplexItem.cc
Unescape View File

@ -36,8 +36,7 @@ const char* TransectStyleComplexItem::_jsonTransectPointsKey = "Tra @@ -36,8 +36,7 @@ const char* TransectStyleComplexItem::_jsonTransectPointsKey = "Tra
const char* TransectStyleComplexItem::_jsonItemsKey = "Items";
const char* TransectStyleComplexItem::_jsonFollowTerrainKey = "FollowTerrain";
const int TransectStyleComplexItem::_terrainQueryTimeoutMsecs = 500;
const double TransectStyleComplexItem::_surveyEdgeIndicator = -10;
const int TransectStyleComplexItem::_terrainQueryTimeoutMsecs = 1000;
TransectStyleComplexItem::TransectStyleComplexItem(Vehicle* vehicle, QString settingsGroup, QObject* parent)
: ComplexMissionItem (vehicle, parent)
@ -77,18 +76,6 @@ TransectStyleComplexItem::TransectStyleComplexItem(Vehicle* vehicle, QString set @@ -77,18 +76,6 @@ TransectStyleComplexItem::TransectStyleComplexItem(Vehicle* vehicle, QString set
connect(_cameraCalc.adjustedFootprintSide(), &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects);
connect(_cameraCalc.adjustedFootprintFrontal(), &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects);
connect(&_turnAroundDistanceFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged);
connect(&_hoverAndCaptureFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged);
connect(&_refly90DegreesFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged);
connect(&_surveyAreaPolygon, &QGCMapPolygon::pathChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged);
connect(&_cameraTriggerInTurnAroundFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged);
connect(_cameraCalc.adjustedFootprintSide(), &Fact::valueChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged);
connect(_cameraCalc.adjustedFootprintFrontal(), &Fact::valueChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged);
connect(this, &TransectStyleComplexItem::followTerrainChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged);
connect(&_terrainAdjustMaxClimbRateFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged);
connect(&_terrainAdjustMaxDescentRateFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged);
connect(&_terrainAdjustToleranceFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged);
connect(&_turnAroundDistanceFact, &Fact::valueChanged, this, &TransectStyleComplexItem::complexDistanceChanged);
connect(&_hoverAndCaptureFact, &Fact::valueChanged, this, &TransectStyleComplexItem::complexDistanceChanged);
connect(&_refly90DegreesFact, &Fact::valueChanged, this, &TransectStyleComplexItem::complexDistanceChanged);
@ -114,8 +101,8 @@ TransectStyleComplexItem::TransectStyleComplexItem(Vehicle* vehicle, QString set @@ -114,8 +101,8 @@ TransectStyleComplexItem::TransectStyleComplexItem(Vehicle* vehicle, QString set
connect(&_surveyAreaPolygon, &QGCMapPolygon::pathChanged, this, &TransectStyleComplexItem::coveredAreaChanged);
connect(this, &TransectStyleComplexItem::transectPointsChanged, this, &TransectStyleComplexItem::complexDistanceChanged);
connect(this, &TransectStyleComplexItem::transectPointsChanged, this, &TransectStyleComplexItem::greatestDistanceToChanged);
connect(this, &TransectStyleComplexItem::visualTransectPointsChanged, this, &TransectStyleComplexItem::complexDistanceChanged);
connect(this, &TransectStyleComplexItem::visualTransectPointsChanged, this, &TransectStyleComplexItem::greatestDistanceToChanged);
}
void TransectStyleComplexItem::_setCameraShots(int cameraShots)
@ -162,7 +149,7 @@ void TransectStyleComplexItem::_save(QJsonObject& complexObject) @@ -162,7 +149,7 @@ void TransectStyleComplexItem::_save(QJsonObject& complexObject)
QJsonValue transectPointsJson;
// Save transects polyline
JsonHelper::saveGeoCoordinateArray(_transectPoints, false /* writeAltitude */, transectPointsJson);
JsonHelper::saveGeoCoordinateArray(_visualTransectPoints, false /* writeAltitude */, transectPointsJson);
innerObject[_jsonTransectPointsKey] = transectPointsJson;
// Save the interal mission items
@ -226,10 +213,13 @@ bool TransectStyleComplexItem::_load(const QJsonObject& complexObject, QString& @@ -226,10 +213,13 @@ bool TransectStyleComplexItem::_load(const QJsonObject& complexObject, QString&
return false;
}
#if 0
// FIXME
// Load transect points
if (!JsonHelper::loadGeoCoordinateArray(innerObject[_jsonTransectPointsKey], false /* altitudeRequired */, _transectPoints, errorString)) {
return false;
}
#endif
// Load generated mission items
_loadedMissionItemsParent = new QObject(this);
@ -277,8 +267,8 @@ bool TransectStyleComplexItem::_load(const QJsonObject& complexObject, QString& @@ -277,8 +267,8 @@ bool TransectStyleComplexItem::_load(const QJsonObject& complexObject, QString&
double TransectStyleComplexItem::greatestDistanceTo(const QGeoCoordinate &other) const
{
double greatestDistance = 0.0;
for (int i=0; i<_transectPoints.count(); i++) {
QGeoCoordinate vertex = _transectPoints[i].value<QGeoCoordinate>();
for (int i=0; i<_visualTransectPoints.count(); i++) {
QGeoCoordinate vertex = _visualTransectPoints[i].value<QGeoCoordinate>();
double distance = vertex.distanceTo(other);
if (distance > greatestDistance) {
greatestDistance = distance;
@ -327,11 +317,6 @@ void TransectStyleComplexItem::_updateCoordinateAltitudes(void) @@ -327,11 +317,6 @@ void TransectStyleComplexItem::_updateCoordinateAltitudes(void)
emit exitCoordinateChanged(exitCoordinate());
}
void TransectStyleComplexItem::_signalLastSequenceNumberChanged(void)
{
emit lastSequenceNumberChanged(lastSequenceNumber());
}
double TransectStyleComplexItem::coveredArea(void) const
{
return _surveyAreaPolygon.area();
@ -355,7 +340,21 @@ bool TransectStyleComplexItem::hoverAndCaptureAllowed(void) const @@ -355,7 +340,21 @@ bool TransectStyleComplexItem::hoverAndCaptureAllowed(void) const
void TransectStyleComplexItem::_rebuildTransects(void)
{
_rebuildTransectsPhase1();
_queryTransectsPathHeightInfo();
// Generate the visuals transect representation
_visualTransectPoints.clear();
foreach (const QList<CoordInfo_t>& transect, _transects) {
foreach (const CoordInfo_t& coordInfo, transect) {
_visualTransectPoints.append(QVariant::fromValue(coordInfo.coord));
}
}
emit visualTransectPointsChanged();
_rebuildTransectsPhase2();
emit lastSequenceNumberChanged(lastSequenceNumber());
}
void TransectStyleComplexItem::_queryTransectsPathHeightInfo(void)
@ -367,89 +366,104 @@ void TransectStyleComplexItem::_queryTransectsPathHeightInfo(void) @@ -367,89 +366,104 @@ void TransectStyleComplexItem::_queryTransectsPathHeightInfo(void)
_terrainPolyPathQuery = NULL;
}
if (_transectPoints.count() > 1) {
if (_transects.count()) {
// We don't actually send the query until this timer times out. This way we only send
// the laset request if we get a bunch in a row.
// the latest request if we get a bunch in a row.
_terrainQueryTimer.start();
}
}
void TransectStyleComplexItem::_reallyQueryTransectsPathHeightInfo(void)
{
if (_transectPoints.count() > 1) {
// Append all transects into a single PolyPath query
QList<QGeoCoordinate> transectPoints;
foreach (const QList<CoordInfo_t>& transect, _transects) {
foreach (const CoordInfo_t& coordInfo, transect) {
transectPoints.append(coordInfo.coord);
}
}
if (transectPoints.count() > 1) {
_terrainPolyPathQuery = new TerrainPolyPathQuery(this);
connect(_terrainPolyPathQuery, &TerrainPolyPathQuery::terrainData, this, &TransectStyleComplexItem::_polyPathTerrainData);
_terrainPolyPathQuery->requestData(_transectPoints);
_terrainPolyPathQuery->requestData(transectPoints);
}
}
void TransectStyleComplexItem::_polyPathTerrainData(bool success, const QList<TerrainPathQuery::PathHeightInfo_t>& rgPathHeightInfo)
{
_transectsPathHeightInfo.clear();
if (success) {
_transectsPathHeightInfo = rgPathHeightInfo;
} else {
_transectsPathHeightInfo.clear();
// Break out into individual transects
int pathHeightIndex = 0;
for (int i=0; i<_transects.count(); i++) {
_transectsPathHeightInfo.append(QList<TerrainPathQuery::PathHeightInfo_t>());
int cPathHeight = _transects[i].count() - 1;
while (cPathHeight-- > 0) {
_transectsPathHeightInfo[i].append(rgPathHeightInfo[pathHeightIndex++]);
}
pathHeightIndex++; // There is an extra on between each transect
}
// Now that we have terrain data we can adjust
_adjustTransectsForTerrain();
}
}
bool TransectStyleComplexItem::readyForSave(void) const
{
// Make sure we have the terrain data we need
return _transectPoints.count() > 1 ? _transectsPathHeightInfo.count() : false;
return _followTerrain ? _transectsPathHeightInfo.count() : true;
}
/// Add altitude values to the standard transect points (whether following terrain or not)
void TransectStyleComplexItem::_adjustTransectPointsForTerrain(void)
void TransectStyleComplexItem::_adjustTransectsForTerrain(void)
{
if (_followTerrain && !readyForSave()) {
qCWarning(TransectStyleComplexItemLog) << "_adjustTransectPointsForTerrain called when terrain data not ready";
qgcApp()->showMessage(tr("INTERNAL ERROR: TransectStyleComplexItem::_adjustTransectPointsForTerrain called when terrain data not ready. Plan will be incorrect."));
return;
}
double requestedAltitude = _cameraCalc.distanceToSurface()->rawValue().toDouble();
qDebug() << _transectPoints.count() << _transectsPathHeightInfo.count();
for (int i=0; i<_transectPoints.count() - 1; i++) {
QGeoCoordinate transectPoint = _transectPoints[i].value<QGeoCoordinate>();
if (_followTerrain) {
if (!readyForSave()) {
qCWarning(TransectStyleComplexItemLog) << "_adjustTransectPointsForTerrain called when terrain data not ready";
qgcApp()->showMessage(tr("INTERNAL ERROR: TransectStyleComplexItem::_adjustTransectPointsForTerrain called when terrain data not ready. Plan will be incorrect."));
return;
}
bool surveyEdgeIndicator = transectPoint.altitude() == _surveyEdgeIndicator;
if (_followTerrain) {
transectPoint.setAltitude(_transectsPathHeightInfo[i].heights[0] + requestedAltitude);
} else {
transectPoint.setAltitude(requestedAltitude);
// First step is add all interstitial points at max resolution
for (int i=0; i<_transects.count(); i++) {
_addInterstitialTerrainPoints(_transects[i], _transectsPathHeightInfo[i]);
}
if (surveyEdgeIndicator) {
// Use to indicate survey edge
transectPoint.setAltitude(-transectPoint.altitude());
for (int i=0; i<_transects.count(); i++) {
_adjustForMaxRates(_transects[i]);
}
_transectPoints[i] = QVariant::fromValue(transectPoint);
}
for (int i=0; i<_transects.count(); i++) {
_adjustForTolerance(_transects[i]);
}
// Take care of last point
QGeoCoordinate transectPoint = _transectPoints.last().value<QGeoCoordinate>();
bool surveyEdgeIndicator = transectPoint.altitude() == _surveyEdgeIndicator;
if (_followTerrain){
transectPoint.setAltitude(_transectsPathHeightInfo.last().heights.last() + requestedAltitude);
emit lastSequenceNumberChanged(lastSequenceNumber());
} else {
transectPoint.setAltitude(requestedAltitude);
}
if (surveyEdgeIndicator) {
// Use to indicate survey edge
transectPoint.setAltitude(-transectPoint.altitude());
}
_transectPoints[_transectPoints.count() - 1] = QVariant::fromValue(transectPoint);
// Not following terrain show just add requested altitude to coords
double requestedAltitude = _cameraCalc.distanceToSurface()->rawValue().toDouble();
for (int i=0; i<_transects.count(); i++) {
QList<CoordInfo_t>& transect = _transects[i];
_addInterstitialTransectsForTerrain();
for (int j=0; j<transect.count(); j++) {
QGeoCoordinate& coord = transect[j].coord;
coord.setAltitude(requestedAltitude);
}
}
}
}
/// Returns the altitude in between the two points on a line.
/// @param precentTowardsTo Example: .25 = twenty five percent along the distance of from to to
double TransectStyleComplexItem::_altitudeBetweenCoords(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord, double percentTowardsTo)
{
double fromAlt = qAbs(fromCoord.altitude());
double toAlt = qAbs(toCoord.altitude());
double fromAlt = fromCoord.altitude();
double toAlt = toCoord.altitude();
double altDiff = toAlt - fromAlt;
return fromAlt + (altDiff * percentTowardsTo);
}
@ -483,55 +497,146 @@ int TransectStyleComplexItem::_maxPathHeight(const TerrainPathQuery::PathHeightI @@ -483,55 +497,146 @@ int TransectStyleComplexItem::_maxPathHeight(const TerrainPathQuery::PathHeightI
return maxIndex;
}
/// Add points in between existing points to account for terrain
void TransectStyleComplexItem::_addInterstitialTransectsForTerrain(void)
void TransectStyleComplexItem::_adjustForMaxRates(QList<CoordInfo_t>& transect)
{
if (!_followTerrain) {
double maxClimbRate = _terrainAdjustMaxClimbRateFact.rawValue().toDouble();
double maxDescentRate = -_terrainAdjustMaxDescentRateFact.rawValue().toDouble();
double flightSpeed = _missionFlightStatus.vehicleSpeed;
if (qIsNaN(flightSpeed) || (maxClimbRate == 0 && maxDescentRate == 0)) {
if (qIsNaN(flightSpeed)) {
qWarning() << "TransectStyleComplexItem::_adjustForMaxRates called with flightSpeed = NaN";
}
return;
}
double requestedAltitude = _cameraCalc.distanceToSurface()->rawValue().toDouble();
// Adjust climb rates
bool climbRateAdjusted;
do {
//qDebug() << "climbrate pass";
climbRateAdjusted = false;
for (int i=0; i<transect.count() - 1; i++) {
QGeoCoordinate& fromCoord = transect[i].coord;
QGeoCoordinate& toCoord = transect[i+1].coord;
double altDifference = toCoord.altitude() - fromCoord.altitude();
double distance = fromCoord.distanceTo(toCoord);
double seconds = distance / flightSpeed;
double climbRate = altDifference / seconds;
//qDebug() << QString("Index:%1 altDifference:%2 distance:%3 seconds:%4 climbRate:%5").arg(i).arg(altDifference).arg(distance).arg(seconds).arg(climbRate);
if (climbRate > 0 && climbRate - maxClimbRate > 0.1) {
double maxAltitudeDelta = maxClimbRate * seconds;
fromCoord.setAltitude(toCoord.altitude() - maxAltitudeDelta);
//qDebug() << "Adjusting";
climbRateAdjusted = true;
}
}
} while (climbRateAdjusted);
// Adjust descent rates
bool descentRateAdjusted;
do {
//qDebug() << "descent rate pass";
descentRateAdjusted = false;
for (int i=1; i<transect.count(); i++) {
QGeoCoordinate& fromCoord = transect[i-1].coord;
QGeoCoordinate& toCoord = transect[i].coord;
double altDifference = toCoord.altitude() - fromCoord.altitude();
double distance = fromCoord.distanceTo(toCoord);
double seconds = distance / flightSpeed;
double descentRate = altDifference / seconds;
//qDebug() << QString("Index:%1 altDifference:%2 distance:%3 seconds:%4 descentRate:%5").arg(i).arg(altDifference).arg(distance).arg(seconds).arg(descentRate);
if (descentRate < 0 && descentRate - maxDescentRate < -0.1) {
double maxAltitudeDelta = maxDescentRate * seconds;
toCoord.setAltitude(fromCoord.altitude() + maxAltitudeDelta);
//qDebug() << "Adjusting";
descentRateAdjusted = true;
}
}
} while (descentRateAdjusted);
}
void TransectStyleComplexItem::_adjustForTolerance(QList<CoordInfo_t>& transect)
{
QList<CoordInfo_t> adjustedPoints;
double tolerance = _terrainAdjustToleranceFact.rawValue().toDouble();
QList<QVariant> terrainAdjustedTransectPoints;
// Check for needed terrain adjust in between the transect points
for (int i=0; i<_transectPoints.count() - 1; i++) {
terrainAdjustedTransectPoints.append(_transectPoints[i]);
int coordIndex = 0;
while (coordIndex < transect.count()) {
const CoordInfo_t& fromCoordInfo = transect[coordIndex];
QGeoCoordinate fromCoord = _transectPoints[i].value<QGeoCoordinate>();
QGeoCoordinate toCoord = _transectPoints[i+1].value<QGeoCoordinate>();
adjustedPoints.append(fromCoordInfo);
const TerrainPathQuery::PathHeightInfo_t& pathHeightInfo = _transectsPathHeightInfo[i];
int cHeights = pathHeightInfo.heights.count();
int lastAddedHeightIndex = 0;
//qDebug() << "cHeights" << cHeights << pathHeightInfo.heights;
// Walk forward until we fall out of tolerence or find a fixed point
while (++coordIndex < transect.count()) {
const CoordInfo_t& toCoordInfo = transect[coordIndex];
if (toCoordInfo.coordType != CoordTypeInteriorTerrainAdded || qAbs(fromCoordInfo.coord.altitude() - toCoordInfo.coord.altitude()) > tolerance) {
adjustedPoints.append(toCoordInfo);
coordIndex++;
break;
}
}
}
#if 0
qDebug() << "_adjustForTolerance";
foreach (const TransectStyleComplexItem::CoordInfo_t& coordInfo, adjustedPoints) {
qDebug() << coordInfo.coordType;
}
#endif
transect = adjustedPoints;
}
void TransectStyleComplexItem::_addInterstitialTerrainPoints(QList<CoordInfo_t>& transect, const QList<TerrainPathQuery::PathHeightInfo_t>& transectPathHeightInfo)
{
QList<CoordInfo_t> adjustedTransect;
adjustedTransect.append(transect.first());
for (int i=0; i<transect.count() - 1; i++) {
CoordInfo_t fromCoordInfo = transect[i];
CoordInfo_t toCoordInfo = transect[i+1];
for (int pathHeightIndex=1; pathHeightIndex<pathHeightInfo.heights.count() - 2; pathHeightIndex++) {
double azimuth = fromCoordInfo.coord.azimuthTo(toCoordInfo.coord);
double distance = fromCoordInfo.coord.distanceTo(toCoordInfo.coord);
const TerrainPathQuery::PathHeightInfo_t& pathHeightInfo = transectPathHeightInfo[i];
double requestedAltitude = _cameraCalc.distanceToSurface()->rawValue().toDouble();
fromCoordInfo.coord.setAltitude(pathHeightInfo.heights.first() + requestedAltitude);
toCoordInfo.coord.setAltitude(pathHeightInfo.heights.last() + requestedAltitude);
int cHeights = pathHeightInfo.heights.count();
for (int pathHeightIndex=1; pathHeightIndex<cHeights - 1; pathHeightIndex++) {
double interstitialTerrainHeight = pathHeightInfo.heights[pathHeightIndex];
double percentTowardsTo = (1.0 / (cHeights - lastAddedHeightIndex)) * (pathHeightIndex - lastAddedHeightIndex);
double interstitialHeight = _altitudeBetweenCoords(fromCoord, toCoord, percentTowardsTo);
double interstitialDeltaOverTerrain = interstitialHeight - interstitialTerrainHeight;
double requestedDeltaOverTerrain = requestedAltitude;
//qDebug() << "interstitialDeltaOverTerrain:requestedDeltaOverTerrain" << interstitialDeltaOverTerrain << requestedDeltaOverTerrain;
if (qAbs(requestedDeltaOverTerrain - interstitialDeltaOverTerrain) > tolerance) {
// We need to add a new point to adjust for terrain
double azimuth = fromCoord.azimuthTo(toCoord);
double distance = fromCoord.distanceTo(toCoord);
QGeoCoordinate interstitialCoord = fromCoord.atDistanceAndAzimuth(distance * percentTowardsTo, azimuth);
interstitialCoord.setAltitude(interstitialTerrainHeight + requestedAltitude);
terrainAdjustedTransectPoints.append(QVariant::fromValue(interstitialCoord));
fromCoord = interstitialCoord;
lastAddedHeightIndex = pathHeightIndex;
//qDebug() << "Added index" << terrainAdjustedTransectPoints.count() - 1;
}
double percentTowardsTo = (1.0 / (cHeights - 1)) * pathHeightIndex;
CoordInfo_t interstitialCoordInfo;
interstitialCoordInfo.coordType = CoordTypeInteriorTerrainAdded;
interstitialCoordInfo.coord = fromCoordInfo.coord.atDistanceAndAzimuth(distance * percentTowardsTo, azimuth);
interstitialCoordInfo.coord.setAltitude(interstitialTerrainHeight + requestedAltitude);
adjustedTransect.append(interstitialCoordInfo);
}
adjustedTransect.append(toCoordInfo);
}
#if 0
qDebug() << "_addInterstitialTerrainPoints";
foreach (const TransectStyleComplexItem::CoordInfo_t& coordInfo, adjustedTransect) {
qDebug() << coordInfo.coordType;
}
terrainAdjustedTransectPoints.append(_transectPoints.last());
#endif
_transectPoints = terrainAdjustedTransectPoints;
transect = adjustedTransect;
}
void TransectStyleComplexItem::setFollowTerrain(bool followTerrain)
@ -541,3 +646,23 @@ void TransectStyleComplexItem::setFollowTerrain(bool followTerrain) @@ -541,3 +646,23 @@ void TransectStyleComplexItem::setFollowTerrain(bool followTerrain)
emit followTerrainChanged(followTerrain);
}
}
int TransectStyleComplexItem::lastSequenceNumber(void) const
{
int itemCount = 0;
foreach (const QList<CoordInfo_t>& rgCoordInfo, _transects) {
itemCount += rgCoordInfo.count();
}
if (_cameraTriggerInTurnAroundFact.rawValue().toBool()) {
// Only one camera start and on camera stop
itemCount += 2;
} else {
// Each transect will have a camera start and stop in it
itemCount += _transects.count() * 2;
}
return _sequenceNumber + itemCount - 1;
}

51
src/MissionManager/TransectStyleComplexItem.h
Unescape View File

@ -39,7 +39,7 @@ public: @@ -39,7 +39,7 @@ public:
Q_PROPERTY(double coveredArea READ coveredArea NOTIFY coveredAreaChanged)
Q_PROPERTY(double cameraMinTriggerInterval READ cameraMinTriggerInterval NOTIFY cameraMinTriggerIntervalChanged)
Q_PROPERTY(bool hoverAndCaptureAllowed READ hoverAndCaptureAllowed CONSTANT)
Q_PROPERTY(QVariantList transectPoints READ transectPoints NOTIFY transectPointsChanged)
Q_PROPERTY(QVariantList visualTransectPoints READ visualTransectPoints NOTIFY visualTransectPointsChanged)
Q_PROPERTY(bool followTerrain READ followTerrain WRITE setFollowTerrain NOTIFY followTerrainChanged)
Q_PROPERTY(Fact* terrainAdjustTolerance READ terrainAdjustTolerance CONSTANT)
@ -48,7 +48,7 @@ public: @@ -48,7 +48,7 @@ public:
QGCMapPolygon* surveyAreaPolygon (void) { return &_surveyAreaPolygon; }
CameraCalc* cameraCalc (void) { return &_cameraCalc; }
QVariantList transectPoints (void) { return _transectPoints; }
QVariantList visualTransectPoints(void) { return _visualTransectPoints; }
Fact* turnAroundDistance (void) { return &_turnAroundDistanceFact; }
Fact* cameraTriggerInTurnAround (void) { return &_cameraTriggerInTurnAroundFact; }
@ -69,7 +69,7 @@ public: @@ -69,7 +69,7 @@ public:
// Overrides from ComplexMissionItem
int lastSequenceNumber (void) const override = 0;
int lastSequenceNumber (void) const final;
QString mapVisualQML (void) const override = 0;
bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) override = 0;
@ -120,19 +120,19 @@ signals: @@ -120,19 +120,19 @@ signals:
void cameraShotsChanged (void);
void timeBetweenShotsChanged (void);
void cameraMinTriggerIntervalChanged(double cameraMinTriggerInterval);
void transectPointsChanged (void);
void visualTransectPointsChanged (void);
void coveredAreaChanged (void);
void followTerrainChanged (bool followTerrain);
protected slots:
virtual void _rebuildTransectsPhase1 (void) = 0;
virtual void _rebuildTransectsPhase2 (void) = 0;
virtual void _rebuildTransectsPhase1 (void) = 0; ///< Rebuilds the _transects array
virtual void _rebuildTransectsPhase2 (void) = 0; ///< Adjust values associated with _transects array
void _setDirty (void);
void _setIfDirty (bool dirty);
void _updateCoordinateAltitudes (void);
void _signalLastSequenceNumberChanged (void);
void _polyPathTerrainData (bool success, const QList<TerrainPathQuery::PathHeightInfo_t>& rgPathHeightInfo);
void _rebuildTransects (void);
protected:
void _save (QJsonObject& saveObject);
@ -142,8 +142,6 @@ protected: @@ -142,8 +142,6 @@ protected:
double _triggerDistance (void) const;
bool _hasTurnaround (void) const;
double _turnaroundDistance (void) const;
void _queryTransectsPathHeightInfo (void);
void _adjustTransectPointsForTerrain (void);
QString _settingsGroup;
int _sequenceNumber;
@ -152,10 +150,23 @@ protected: @@ -152,10 +150,23 @@ protected:
QGeoCoordinate _exitCoordinate;
QGCMapPolygon _surveyAreaPolygon;
QVariantList _transectPoints;
QList<TerrainPathQuery::PathHeightInfo_t> _transectsPathHeightInfo;
TerrainPolyPathQuery* _terrainPolyPathQuery;
QTimer _terrainQueryTimer;
enum CoordType {
CoordTypeInterior,
CoordTypeInteriorTerrainAdded,
CoordTypeSurveyEdge,
CoordTypeTurnaround
};
typedef struct {
QGeoCoordinate coord;
CoordType coordType;
} CoordInfo_t;
QVariantList _visualTransectPoints;
QList<QList<CoordInfo_t>> _transects;
QList<QList<TerrainPathQuery::PathHeightInfo_t>> _transectsPathHeightInfo;
TerrainPolyPathQuery* _terrainPolyPathQuery;
QTimer _terrainQueryTimer;
bool _ignoreRecalc;
double _complexDistance;
@ -186,14 +197,16 @@ protected: @@ -186,14 +197,16 @@ protected:
static const char* _jsonFollowTerrainKey;
static const int _terrainQueryTimeoutMsecs;
static const double _surveyEdgeIndicator; ///< Altitude value in _transectPoints which indicates survey entry
private slots:
void _rebuildTransects (void);
void _reallyQueryTransectsPathHeightInfo (void);
void _reallyQueryTransectsPathHeightInfo(void);
private:
void _addInterstitialTransectsForTerrain (void);
double _altitudeBetweenCoords (const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord, double percentTowardsTo);
int _maxPathHeight (const TerrainPathQuery::PathHeightInfo_t& pathHeightInfo, int fromIndex, int toIndex, double& maxHeight);
void _queryTransectsPathHeightInfo (void);
void _adjustTransectsForTerrain (void);
void _addInterstitialTerrainPoints (QList<CoordInfo_t>& transect, const QList<TerrainPathQuery::PathHeightInfo_t>& transectPathHeightInfo);
void _adjustForMaxRates (QList<CoordInfo_t>& transect);
void _adjustForTolerance (QList<CoordInfo_t>& transect);
double _altitudeBetweenCoords (const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord, double percentTowardsTo);
int _maxPathHeight (const TerrainPathQuery::PathHeightInfo_t& pathHeightInfo, int fromIndex, int toIndex, double& maxHeight);
};

2
src/MissionManager/TransectStyleComplexItemTest.cc
Unescape View File

@ -31,7 +31,7 @@ void TransectStyleComplexItemTest::init(void) @@ -31,7 +31,7 @@ void TransectStyleComplexItemTest::init(void)
_rgSignals[cameraShotsChangedIndex] = SIGNAL(cameraShotsChanged());
_rgSignals[timeBetweenShotsChangedIndex] = SIGNAL(timeBetweenShotsChanged());
_rgSignals[cameraMinTriggerIntervalChangedIndex] = SIGNAL(cameraMinTriggerIntervalChanged(double));
_rgSignals[transectPointsChangedIndex] = SIGNAL(transectPointsChanged());
_rgSignals[visualTransectPointsChangedIndex] = SIGNAL(visualTransectPointsChanged());
_rgSignals[coveredAreaChangedIndex] = SIGNAL(coveredAreaChanged());
_rgSignals[dirtyChangedIndex] = SIGNAL(dirtyChanged(bool));
_rgSignals[complexDistanceChangedIndex] = SIGNAL(complexDistanceChanged());

23
src/MissionManager/TransectStyleComplexItemTest.h
Unescape View File

@ -42,7 +42,7 @@ private: @@ -42,7 +42,7 @@ private:
cameraShotsChangedIndex = 0,
timeBetweenShotsChangedIndex,
cameraMinTriggerIntervalChangedIndex,
transectPointsChangedIndex,
visualTransectPointsChangedIndex,
coveredAreaChangedIndex,
// These signals are from ComplexItem
dirtyChangedIndex,
@ -56,18 +56,18 @@ private: @@ -56,18 +56,18 @@ private:
enum {
// These signals are from TransectStyleComplexItem
cameraShotsChangedMask = 1 << cameraShotsChangedIndex,
timeBetweenShotsChangedMask = 1 << timeBetweenShotsChangedIndex,
cameraMinTriggerIntervalChangedMask = 1 << cameraMinTriggerIntervalChangedIndex,
transectPointsChangedMask = 1 << transectPointsChangedIndex,
coveredAreaChangedMask = 1 << coveredAreaChangedIndex,
cameraShotsChangedMask = 1 << cameraShotsChangedIndex,
timeBetweenShotsChangedMask = 1 << timeBetweenShotsChangedIndex,
cameraMinTriggerIntervalChangedMask = 1 << cameraMinTriggerIntervalChangedIndex,
visualTransectPointsChangedMask = 1 << visualTransectPointsChangedIndex,
coveredAreaChangedMask = 1 << coveredAreaChangedIndex,
// These signals are from ComplexItem
dirtyChangedMask = 1 << dirtyChangedIndex,
complexDistanceChangedMask = 1 << complexDistanceChangedIndex,
greatestDistanceToChangedMask = 1 << greatestDistanceToChangedIndex,
additionalTimeDelayChangedMask = 1 << additionalTimeDelayChangedIndex,
dirtyChangedMask = 1 << dirtyChangedIndex,
complexDistanceChangedMask = 1 << complexDistanceChangedIndex,
greatestDistanceToChangedMask = 1 << greatestDistanceToChangedIndex,
additionalTimeDelayChangedMask = 1 << additionalTimeDelayChangedIndex,
// These signals are from VisualMissionItem
lastSequenceNumberChangedMask = 1 << lastSequenceNumberChangedIndex,
lastSequenceNumberChangedMask = 1 << lastSequenceNumberChangedIndex,
};
static const size_t _cSignals = maxSignalIndex;
@ -87,7 +87,6 @@ public: @@ -87,7 +87,6 @@ public:
TransectStyleItem(Vehicle* vehicle, QObject* parent = NULL);
// Overrides from ComplexMissionItem
int lastSequenceNumber (void) const final { return _sequenceNumber; }
QString mapVisualQML (void) const final { return QString(); }
bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final { Q_UNUSED(complexObject); Q_UNUSED(sequenceNumber); Q_UNUSED(errorString); return false; }

19
src/MissionManager/VisualMissionItem.cc
Unescape View File

@ -38,15 +38,7 @@ VisualMissionItem::VisualMissionItem(Vehicle* vehicle, QObject* parent) @@ -38,15 +38,7 @@ VisualMissionItem::VisualMissionItem(Vehicle* vehicle, QObject* parent)
, _lastLatTerrainQuery (0)
, _lastLonTerrainQuery (0)
{
// Don't get terrain altitude information for submarines or boards
if (_vehicle->vehicleType() != MAV_TYPE_SUBMARINE && _vehicle->vehicleType() != MAV_TYPE_SURFACE_BOAT) {
_updateTerrainTimer.setInterval(500);
_updateTerrainTimer.setSingleShot(true);
connect(&_updateTerrainTimer, &QTimer::timeout, this, &VisualMissionItem::_reallyUpdateTerrainAltitude);
connect(this, &VisualMissionItem::coordinateChanged, this, &VisualMissionItem::_updateTerrainAltitude);
}
_commonInit();
}
VisualMissionItem::VisualMissionItem(const VisualMissionItem& other, QObject* parent)
@ -63,8 +55,17 @@ VisualMissionItem::VisualMissionItem(const VisualMissionItem& other, QObject* pa @@ -63,8 +55,17 @@ VisualMissionItem::VisualMissionItem(const VisualMissionItem& other, QObject* pa
{
*this = other;
_commonInit();
}
void VisualMissionItem::_commonInit(void)
{
// Don't get terrain altitude information for submarines or boats
if (_vehicle->vehicleType() != MAV_TYPE_SUBMARINE && _vehicle->vehicleType() != MAV_TYPE_SURFACE_BOAT) {
_updateTerrainTimer.setInterval(500);
_updateTerrainTimer.setSingleShot(true);
connect(&_updateTerrainTimer, &QTimer::timeout, this, &VisualMissionItem::_reallyUpdateTerrainAltitude);
connect(this, &VisualMissionItem::coordinateChanged, this, &VisualMissionItem::_updateTerrainAltitude);
}
}

2
src/MissionManager/VisualMissionItem.h
Unescape View File

@ -214,6 +214,8 @@ private slots: @@ -214,6 +214,8 @@ private slots:
void _terrainDataReceived (bool success, QList<double> heights);
private:
void _commonInit(void);
QTimer _updateTerrainTimer;
double _lastLatTerrainQuery;
double _lastLonTerrainQuery;

2
src/PlanView/CorridorScanMapVisual.qml
Unescape View File

@ -119,7 +119,7 @@ Item { @@ -119,7 +119,7 @@ Item {
MapPolyline {
line.color: "white"
line.width: 2
path: _missionItem.transectPoints
path: _missionItem.visualTransectPoints
}
}
}

15
src/Terrain/TerrainQuery.cc
Unescape View File

@ -80,8 +80,8 @@ void TerrainAirMapQuery::requestCarpetHeights(const QGeoCoordinate& swCoord, con @@ -80,8 +80,8 @@ void TerrainAirMapQuery::requestCarpetHeights(const QGeoCoordinate& swCoord, con
void TerrainAirMapQuery::_sendQuery(const QString& path, const QUrlQuery& urlQuery)
{
QUrl url(QStringLiteral("https://api.airmap.com/elevation/v1/ele") + path);
url.setQuery(urlQuery);
qCDebug(TerrainQueryLog) << "_sendQuery" << url;
url.setQuery(urlQuery);
QNetworkRequest request(url);
@ -133,7 +133,7 @@ void TerrainAirMapQuery::_requestFinished(void) @@ -133,7 +133,7 @@ void TerrainAirMapQuery::_requestFinished(void)
// Send back data
const QJsonValue& jsonData = rootObject["data"];
qCDebug(TerrainQueryLog) << "_requestFinished" << jsonData;
qCDebug(TerrainQueryLog) << "_requestFinished sucess";
switch (_queryMode) {
case QueryModeCoordinates:
emit _parseCoordinateData(jsonData);
@ -227,7 +227,6 @@ TerrainAtCoordinateBatchManager::TerrainAtCoordinateBatchManager(void) @@ -227,7 +227,6 @@ TerrainAtCoordinateBatchManager::TerrainAtCoordinateBatchManager(void)
void TerrainAtCoordinateBatchManager::addQuery(TerrainAtCoordinateQuery* terrainAtCoordinateQuery, const QList<QGeoCoordinate>& coordinates)
{
if (coordinates.length() > 0) {
qCDebug(TerrainQueryLog) << "addQuery: TerrainAtCoordinateQuery:coordinates.count" << terrainAtCoordinateQuery << coordinates.count();
connect(terrainAtCoordinateQuery, &TerrainAtCoordinateQuery::destroyed, this, &TerrainAtCoordinateBatchManager::_queryObjectDestroyed);
QueuedRequestInfo_t queuedRequestInfo = { terrainAtCoordinateQuery, coordinates };
_requestQueue.append(queuedRequestInfo);
@ -255,14 +254,18 @@ void TerrainAtCoordinateBatchManager::_sendNextBatch(void) @@ -255,14 +254,18 @@ void TerrainAtCoordinateBatchManager::_sendNextBatch(void)
// Convert coordinates to point strings for json query
QList<QGeoCoordinate> coords;
int requestQueueAdded = 0;
foreach (const QueuedRequestInfo_t& requestInfo, _requestQueue) {
SentRequestInfo_t sentRequestInfo = { requestInfo.terrainAtCoordinateQuery, false, requestInfo.coordinates.count() };
qCDebug(TerrainQueryLog) << "Building request: coordinate count" << requestInfo.coordinates.count();
_sentRequests.append(sentRequestInfo);
coords += requestInfo.coordinates;
requestQueueAdded++;
if (coords.count() > 50) {
break;
}
}
_requestQueue.clear();
qCDebug(TerrainQueryLog) << "Built request: coordinate count" << coords.count();
_requestQueue = _requestQueue.mid(requestQueueAdded);
_terrainQuery.requestCoordinateHeights(coords);
_state = State::Downloading;

Write Preview
Loading…
Cancel
Save