diff --git a/src/MissionManager/TransectStyleComplexItem.cc b/src/MissionManager/TransectStyleComplexItem.cc index 31f483c3ee77fdbbfd8f93680c082405f34bff97..0366414f524eaf168b3c3868c9ca2bbd4a9daf20 100644 --- a/src/MissionManager/TransectStyleComplexItem.cc +++ b/src/MissionManager/TransectStyleComplexItem.cc @@ -34,6 +34,7 @@ const char* TransectStyleComplexItem::_jsonTransectStyleComplexItemKey = "Tra const char* TransectStyleComplexItem::_jsonCameraCalcKey = "CameraCalc"; const char* TransectStyleComplexItem::_jsonTransectPointsKey = "TransectPoints"; const char* TransectStyleComplexItem::_jsonItemsKey = "Items"; +const char* TransectStyleComplexItem::_jsonFollowTerrainKey = "FollowTerrain"; const int TransectStyleComplexItem::_terrainQueryTimeoutMsecs = 500; const double TransectStyleComplexItem::_surveyEdgeIndicator = -10; @@ -67,6 +68,10 @@ TransectStyleComplexItem::TransectStyleComplexItem(Vehicle* vehicle, QString set connect(&_turnAroundDistanceFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); connect(&_hoverAndCaptureFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); connect(&_refly90DegreesFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); + connect(this, &TransectStyleComplexItem::followTerrainChanged, this, &TransectStyleComplexItem::_rebuildTransects); + connect(&_terrainAdjustMaxClimbRateFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); + connect(&_terrainAdjustMaxDescentRateFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); + connect(&_terrainAdjustToleranceFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); connect(&_surveyAreaPolygon, &QGCMapPolygon::pathChanged, this, &TransectStyleComplexItem::_rebuildTransects); connect(&_cameraTriggerInTurnAroundFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); connect(_cameraCalc.adjustedFootprintSide(), &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); @@ -79,6 +84,10 @@ TransectStyleComplexItem::TransectStyleComplexItem(Vehicle* vehicle, QString set 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); @@ -94,6 +103,10 @@ TransectStyleComplexItem::TransectStyleComplexItem(Vehicle* vehicle, QString set connect(&_cameraTriggerInTurnAroundFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_setDirty); connect(&_hoverAndCaptureFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_setDirty); connect(&_refly90DegreesFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_setDirty); + connect(this, &TransectStyleComplexItem::followTerrainChanged, this, &TransectStyleComplexItem::_setDirty); + connect(&_terrainAdjustMaxClimbRateFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_setDirty); + connect(&_terrainAdjustMaxDescentRateFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_setDirty); + connect(&_terrainAdjustToleranceFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_setDirty); connect(&_surveyAreaPolygon, &QGCMapPolygon::pathChanged, this, &TransectStyleComplexItem::_setDirty); connect(&_surveyAreaPolygon, &QGCMapPolygon::dirtyChanged, this, &TransectStyleComplexItem::_setIfDirty); @@ -134,6 +147,13 @@ void TransectStyleComplexItem::_save(QJsonObject& complexObject) innerObject[cameraTriggerInTurnAroundName] = _cameraTriggerInTurnAroundFact.rawValue().toBool(); innerObject[hoverAndCaptureName] = _hoverAndCaptureFact.rawValue().toBool(); innerObject[refly90DegreesName] = _refly90DegreesFact.rawValue().toBool(); + innerObject[_jsonFollowTerrainKey] = _followTerrain; + + if (_followTerrain) { + innerObject[terrainAdjustToleranceName] = _terrainAdjustToleranceFact.rawValue().toDouble(); + innerObject[terrainAdjustMaxClimbRateName] = _terrainAdjustMaxClimbRateFact.rawValue().toDouble(); + innerObject[terrainAdjustMaxDescentRateName] = _terrainAdjustMaxDescentRateFact.rawValue().toDouble(); + } QJsonObject cameraCalcObject; _cameraCalc.save(cameraCalcObject); @@ -183,6 +203,14 @@ bool TransectStyleComplexItem::_load(const QJsonObject& complexObject, QString& // The TransectStyleComplexItem is a sub-object of the main complex item object QJsonObject innerObject = complexObject[_jsonTransectStyleComplexItemKey].toObject(); + if (innerObject.contains(JsonHelper::jsonVersionKey)) { + int version = innerObject[JsonHelper::jsonVersionKey].toInt(); + if (version != 1) { + errorString = tr("TransectStyleComplexItem version %2 not supported").arg(version); + return false; + } + } + QList innerKeyInfoList = { { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, { turnAroundDistanceName, QJsonValue::Double, true }, @@ -192,17 +220,12 @@ bool TransectStyleComplexItem::_load(const QJsonObject& complexObject, QString& { _jsonCameraCalcKey, QJsonValue::Object, true }, { _jsonTransectPointsKey, QJsonValue::Array, true }, { _jsonItemsKey, QJsonValue::Array, true }, + { _jsonFollowTerrainKey, QJsonValue::Bool, true }, }; if (!JsonHelper::validateKeys(innerObject, innerKeyInfoList, errorString)) { return false; } - int version = innerObject[JsonHelper::jsonVersionKey].toInt(); - if (version != 1) { - errorString = tr("TransectStyleComplexItem version %2 not supported").arg(version); - return false; - } - // Load transect points if (!JsonHelper::loadGeoCoordinateArray(innerObject[_jsonTransectPointsKey], false /* altitudeRequired */, _transectPoints, errorString)) { return false; @@ -230,7 +253,23 @@ bool TransectStyleComplexItem::_load(const QJsonObject& complexObject, QString& _turnAroundDistanceFact.setRawValue (innerObject[turnAroundDistanceName].toDouble()); _cameraTriggerInTurnAroundFact.setRawValue (innerObject[cameraTriggerInTurnAroundName].toBool()); _hoverAndCaptureFact.setRawValue (innerObject[hoverAndCaptureName].toBool()); - _hoverAndCaptureFact.setRawValue (innerObject[refly90DegreesName].toBool()); + _refly90DegreesFact.setRawValue (innerObject[refly90DegreesName].toBool()); + _followTerrain = innerObject[_jsonFollowTerrainKey].toBool(); + + if (_followTerrain) { + QList followTerrainKeyInfoList = { + { terrainAdjustToleranceName, QJsonValue::Double, true }, + { terrainAdjustMaxClimbRateName, QJsonValue::Double, true }, + { terrainAdjustMaxDescentRateName, QJsonValue::Double, true }, + }; + if (!JsonHelper::validateKeys(innerObject, followTerrainKeyInfoList, errorString)) { + return false; + } + + _terrainAdjustToleranceFact.setRawValue (innerObject[terrainAdjustToleranceName].toDouble()); + _terrainAdjustMaxClimbRateFact.setRawValue (innerObject[terrainAdjustMaxClimbRateName].toDouble()); + _terrainAdjustMaxDescentRateFact.setRawValue (innerObject[terrainAdjustMaxDescentRateName].toDouble()); + } return true; } @@ -359,6 +398,7 @@ bool TransectStyleComplexItem::readyForSave(void) const return _transectPoints.count() > 1 ? _transectsPathHeightInfo.count() : false; } +/// Add altitude values to the standard transect points (whether following terrain or not) void TransectStyleComplexItem::_adjustTransectPointsForTerrain(void) { if (_followTerrain && !readyForSave()) { @@ -367,16 +407,18 @@ void TransectStyleComplexItem::_adjustTransectPointsForTerrain(void) return; } - double altitude = _cameraCalc.distanceToSurface()->rawValue().toDouble(); + double requestedAltitude = _cameraCalc.distanceToSurface()->rawValue().toDouble(); + qDebug() << _transectPoints.count() << _transectsPathHeightInfo.count(); for (int i=0; i<_transectPoints.count() - 1; i++) { + qDebug() << _transectsPathHeightInfo[i].heights.count(); QGeoCoordinate transectPoint = _transectPoints[i].value(); bool surveyEdgeIndicator = transectPoint.altitude() == _surveyEdgeIndicator; if (_followTerrain) { - transectPoint.setAltitude(_transectsPathHeightInfo[i].heights[0] + altitude); + transectPoint.setAltitude(_transectsPathHeightInfo[i].heights[0] + requestedAltitude); } else { - transectPoint.setAltitude(altitude); + transectPoint.setAltitude(requestedAltitude); } if (surveyEdgeIndicator) { // Use to indicate survey edge @@ -390,15 +432,107 @@ void TransectStyleComplexItem::_adjustTransectPointsForTerrain(void) QGeoCoordinate transectPoint = _transectPoints.last().value(); bool surveyEdgeIndicator = transectPoint.altitude() == _surveyEdgeIndicator; if (_followTerrain){ - transectPoint.setAltitude(_transectsPathHeightInfo.last().heights.last() + altitude); + transectPoint.setAltitude(_transectsPathHeightInfo.last().heights.last() + requestedAltitude); } else { - transectPoint.setAltitude(altitude); + transectPoint.setAltitude(requestedAltitude); } if (surveyEdgeIndicator) { // Use to indicate survey edge transectPoint.setAltitude(-transectPoint.altitude()); } _transectPoints[_transectPoints.count() - 1] = QVariant::fromValue(transectPoint); + + _addInterstitialTransectsForTerrain(); +} + +/// 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 altDiff = toAlt - fromAlt; + return fromAlt + (altDiff * percentTowardsTo); +} + +/// Determine the maximum height within the PathHeightInfo +/// @param fromIndex First height index to consider +/// @param fromIndex Last height index to consider +/// @param[out] maxHeight Maximum height +/// @return index within PathHeightInfo_t.heights which contains maximum height, -1 no height data in between from and to indices +int TransectStyleComplexItem::_maxPathHeight(const TerrainPathQuery::PathHeightInfo_t& pathHeightInfo, int fromIndex, int toIndex, double& maxHeight) +{ + maxHeight = qQNaN(); + + if (toIndex - fromIndex < 2) { + return -1; + } + + fromIndex++; + toIndex--; + + int maxIndex = fromIndex; + maxHeight = pathHeightInfo.heights[fromIndex]; + + for (int i=fromIndex; i<=toIndex; i++) { + if (pathHeightInfo.heights[i] > maxHeight) { + maxIndex = i; + maxHeight = pathHeightInfo.heights[i]; + } + } + + return maxIndex; +} + +/// Add points in between existing points to account for terrain +void TransectStyleComplexItem::_addInterstitialTransectsForTerrain(void) +{ + if (!_followTerrain) { + return; + } + + double requestedAltitude = _cameraCalc.distanceToSurface()->rawValue().toDouble(); + double tolerance = _terrainAdjustToleranceFact.rawValue().toDouble(); + QList terrainAdjustedTransectPoints; + + // Check for needed terrain adjust in between the transect points + for (int i=0; i<_transectPoints.count() - 1; i++) { + terrainAdjustedTransectPoints.append(_transectPoints[i]); + + QGeoCoordinate fromCoord = _transectPoints[i].value(); + QGeoCoordinate toCoord = _transectPoints[i+1].value(); + + const TerrainPathQuery::PathHeightInfo_t& pathHeightInfo = _transectsPathHeightInfo[i]; + int cHeights = pathHeightInfo.heights.count(); + int lastAddedHeightIndex = 0; + //qDebug() << "cHeights" << cHeights << pathHeightInfo.heights; + + for (int pathHeightIndex=1; pathHeightIndex 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; + } + } + } + terrainAdjustedTransectPoints.append(_transectPoints.last()); + + _transectPoints = terrainAdjustedTransectPoints; } void TransectStyleComplexItem::setFollowTerrain(bool followTerrain) diff --git a/src/MissionManager/TransectStyleComplexItem.h b/src/MissionManager/TransectStyleComplexItem.h index 46597ccb7966e25c2deeb68f8dbaa0cb236e0596..e4201efea9391c2b6e074babb81aff81a9299980 100644 --- a/src/MissionManager/TransectStyleComplexItem.h +++ b/src/MissionManager/TransectStyleComplexItem.h @@ -183,6 +183,7 @@ protected: static const char* _jsonTransectStyleComplexItemKey; static const char* _jsonTransectPointsKey; static const char* _jsonItemsKey; + static const char* _jsonFollowTerrainKey; static const int _terrainQueryTimeoutMsecs; static const double _surveyEdgeIndicator; ///< Altitude value in _transectPoints which indicates survey entry @@ -191,4 +192,8 @@ private slots: void _rebuildTransects (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); }; diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 9ad225d6a9e48914c7d531b5aaf8cf026421bdc5..53a381b81ebe55050b5bea5472e820a18408419c 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -35,9 +35,15 @@ QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "MockLinkVerboseLog") // Vehicle position is set close to default Gazebo vehicle location. This allows for multi-vehicle // testing of a gazebo vehicle and a mocklink vehicle -double MockLink::_defaultVehicleLatitude = 47.397f; -double MockLink::_defaultVehicleLongitude = 8.5455f; -double MockLink::_defaultVehicleAltitude = 488.056f; +#if 1 +double MockLink::_defaultVehicleLatitude = 47.397; +double MockLink::_defaultVehicleLongitude = 8.5455; +double MockLink::_defaultVehicleAltitude = 488.056; +#else +double MockLink::_defaultVehicleLatitude = 47.6333022928789; +double MockLink::_defaultVehicleLongitude = -122.08833157994995; +double MockLink::_defaultVehicleAltitude = 19.0; +#endif int MockLink::_nextVehicleSystemId = 128; const char* MockLink::_failParam = "COM_FLTMODE6";