From 1d3e7cae0aab565e57aa084448515c53aeda37d0 Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Fri, 23 Jun 2017 12:53:58 -0700 Subject: [PATCH] Flight time calc for hover and capture --- src/MissionManager/ComplexMissionItem.h | 6 +- src/MissionManager/MissionController.cc | 98 ++++++++++++++----------- src/MissionManager/MissionController.h | 3 + src/MissionManager/SurveyMissionItem.cc | 9 ++- src/MissionManager/SurveyMissionItem.h | 4 + 5 files changed, 76 insertions(+), 44 deletions(-) diff --git a/src/MissionManager/ComplexMissionItem.h b/src/MissionManager/ComplexMissionItem.h index 93c3f1a0f..caf83426a 100644 --- a/src/MissionManager/ComplexMissionItem.h +++ b/src/MissionManager/ComplexMissionItem.h @@ -26,6 +26,9 @@ public: /// @return The distance covered the complex mission item in meters. virtual double complexDistance(void) const = 0; + /// @return Amount of additional time delay in seconds needed to fly the complex item + virtual double additionalTimeDelay(void) const { return 0; } + /// Load the complex mission item from Json /// @param complexObject Complex mission item json object /// @param sequenceNumber Sequence number for first MISSION_ITEM in survey @@ -42,7 +45,8 @@ public: static const char* jsonComplexItemTypeKey; signals: - void complexDistanceChanged(double complexDistance); + void complexDistanceChanged (double complexDistance); + void additionalTimeDelayChanged (double additionalTimeDelay); }; #endif diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index d6223fc8d..b00d9699e 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -1017,6 +1017,53 @@ void MissionController::_addCruiseTime(double cruiseTime, double cruiseDistance, _updateBatteryInfo(waypointIndex); } +/// Adds additional time to a mission as specified by the command +void MissionController::_addCommandTimeDelay(SimpleMissionItem* simpleItem, bool vtolInHover) +{ + double seconds = 0; + + if (!simpleItem) { + return; + } + + // This routine is currently quite minimal and only handles the simple cases. + switch ((int)simpleItem->command()) { + case MAV_CMD_NAV_WAYPOINT: + case MAV_CMD_CONDITION_DELAY: + seconds = simpleItem->missionItem().param1(); + break; + } + + _addTimeDistance(vtolInHover, 0, 0, seconds, 0, -1); +} + +/// Adds the specified time to the appropriate hover or cruise time values. +/// @param vtolInHover true: vtol is currrent in hover mode +/// @param hoverTime Amount of time tp add to hover +/// @param cruiseTime Amount of time to add to cruise +/// @param extraTime Amount of additional time to add to hover/cruise +/// @param seqNum Sequence number of waypoint for these values, -1 for no waypoint associated +void MissionController::_addTimeDistance(bool vtolInHover, double hoverTime, double cruiseTime, double extraTime, double distance, int seqNum) +{ + if (_controllerVehicle->vtol()) { + if (vtolInHover) { + _addHoverTime(hoverTime, distance, seqNum); + _addHoverTime(extraTime, 0, -1); + } else { + _addCruiseTime(cruiseTime, distance, seqNum); + _addCruiseTime(extraTime, 0, -1); + } + } else { + if (_controllerVehicle->multiRotor()) { + _addHoverTime(hoverTime, distance, seqNum); + _addHoverTime(extraTime, 0, -1); + } else { + _addCruiseTime(cruiseTime, distance, seqNum); + _addCruiseTime(extraTime, 0, -1); + } + } +} + void MissionController::_recalcMissionFlightStatus() { if (!_visualItems->count()) { @@ -1137,6 +1184,9 @@ void MissionController::_recalcMissionFlightStatus() } } + // Check for command specific time delays + _addCommandTimeDelay(simpleItem, vtolInHover); + if (item->specifiesCoordinate()) { // Update vehicle yaw assuming direction to next waypoint if (item != lastCoordinateItem) { @@ -1178,19 +1228,7 @@ void MissionController::_recalcMissionFlightStatus() // Calculate time/distance double hoverTime = distance / _missionFlightStatus.hoverSpeed; double cruiseTime = distance / _missionFlightStatus.cruiseSpeed; - if (_controllerVehicle->vtol()) { - if (vtolInHover) { - _addHoverTime(hoverTime, distance, item->sequenceNumber()); - } else { - _addCruiseTime(cruiseTime, distance, item->sequenceNumber()); - } - } else { - if (_controllerVehicle->multiRotor()) { - _addHoverTime(hoverTime, distance, item->sequenceNumber()); - } else { - _addCruiseTime(cruiseTime, distance, item->sequenceNumber()); - } - } + _addTimeDistance(vtolInHover, hoverTime, cruiseTime, 0, distance, item->sequenceNumber()); } if (complexItem) { @@ -1200,19 +1238,8 @@ void MissionController::_recalcMissionFlightStatus() double hoverTime = distance / _missionFlightStatus.hoverSpeed; double cruiseTime = distance / _missionFlightStatus.cruiseSpeed; - if (_controllerVehicle->vtol()) { - if (vtolInHover) { - _addHoverTime(hoverTime, distance, item->sequenceNumber()); - } else { - _addCruiseTime(cruiseTime, distance, item->sequenceNumber()); - } - } else { - if (_controllerVehicle->multiRotor()) { - _addHoverTime(hoverTime, distance, item->sequenceNumber()); - } else { - _addCruiseTime(cruiseTime, distance, item->sequenceNumber()); - } - } + double extraTime = complexItem->additionalTimeDelay(); + _addTimeDistance(vtolInHover, hoverTime, cruiseTime, extraTime, distance, item->sequenceNumber()); } item->setMissionFlightStatus(_missionFlightStatus); @@ -1231,21 +1258,7 @@ void MissionController::_recalcMissionFlightStatus() double hoverTime = distance / _missionFlightStatus.hoverSpeed; double cruiseTime = distance / _missionFlightStatus.cruiseSpeed; double landTime = qAbs(altDifference) / _appSettings->offlineEditingDescentSpeed()->rawValue().toDouble(); - if (_controllerVehicle->vtol()) { - if (vtolInHover) { - _addHoverTime(hoverTime, distance, -1); - _addHoverTime(landTime, 0, -1); - } else { - _addCruiseTime(cruiseTime, distance, -1); - } - } else { - if (_controllerVehicle->multiRotor()) { - _addHoverTime(hoverTime, distance, -1); - _addHoverTime(landTime, 0, -1); - } else { - _addCruiseTime(cruiseTime, distance, -1); - } - } + _addTimeDistance(vtolInHover, hoverTime, cruiseTime, distance, landTime, -1); } if (_missionFlightStatus.mAhBattery != 0 && _missionFlightStatus.batteryChangePoint == -1) { @@ -1416,7 +1429,8 @@ void MissionController::_initVisualItem(VisualMissionItem* visualItem) } else { ComplexMissionItem* complexItem = qobject_cast(visualItem); if (complexItem) { - connect(complexItem, &ComplexMissionItem::complexDistanceChanged, this, &MissionController::_recalcMissionFlightStatus); + connect(complexItem, &ComplexMissionItem::complexDistanceChanged, this, &MissionController::_recalcMissionFlightStatus); + connect(complexItem, &ComplexMissionItem::additionalTimeDelayChanged, this, &MissionController::_recalcMissionFlightStatus); } else { qWarning() << "ComplexMissionItem not found"; } diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 9a325aebf..b5d2e1020 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -25,6 +25,7 @@ class MissionItem; class MissionSettingsItem; class AppSettings; class MissionManager; +class SimpleMissionItem; Q_DECLARE_LOGGING_CATEGORY(MissionControllerLog) @@ -210,6 +211,8 @@ private: bool _loadItemsFromJson(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString); void _initLoadedVisualItems(QmlObjectListModel* loadedVisualItems); void _addWaypointLineSegment(CoordVectHashTable& prevItemPairHashTable, VisualItemPair& pair); + void _addCommandTimeDelay(SimpleMissionItem* simpleItem, bool vtolInHover); + void _addTimeDistance(bool vtolInHover, double hoverTime, double cruiseTime, double extraTime, double distance, int seqNum); private: MissionManager* _missionManager; diff --git a/src/MissionManager/SurveyMissionItem.cc b/src/MissionManager/SurveyMissionItem.cc index 1aea5f02a..cf193a81f 100644 --- a/src/MissionManager/SurveyMissionItem.cc +++ b/src/MissionManager/SurveyMissionItem.cc @@ -78,6 +78,7 @@ SurveyMissionItem::SurveyMissionItem(Vehicle* vehicle, QObject* parent) , _cameraOrientationFixed(false) , _missionCommandCount(0) , _refly90Degrees(false) + , _additionalFlightDelaySeconds(0) , _ignoreRecalc(false) , _surveyDistance(0.0) , _cameraShots(0) @@ -675,6 +676,7 @@ void SurveyMissionItem::_generateGrid(void) _simpleGridPoints.clear(); _transectSegments.clear(); _reflyTransectSegments.clear(); + _additionalFlightDelaySeconds = 0; QList polygonPoints; QList> transectSegments; @@ -737,6 +739,11 @@ void SurveyMissionItem::_generateGrid(void) } _setCameraShots(cameraShots); + if (_hoverAndCaptureEnabled()) { + _additionalFlightDelaySeconds = cameraShots * _hoverAndCaptureDelaySeconds; + } + emit additionalTimeDelayChanged(_additionalFlightDelaySeconds); + emit gridPointsChanged(); // Determine command count for lastSequenceNumber @@ -1108,7 +1115,7 @@ int SurveyMissionItem::_appendWaypointToMission(QList& items, int MissionItem* item = new MissionItem(seqNum++, MAV_CMD_NAV_WAYPOINT, altitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, - cameraTrigger == CameraTriggerHoverAndCapture ? 1 : 0, // Hold time (1 second for hover and capture to settle vehicle before image is taken) + cameraTrigger == CameraTriggerHoverAndCapture ? _hoverAndCaptureDelaySeconds : 0, // Hold time (delay for hover and capture to settle vehicle before image is taken) 0.0, 0.0, std::numeric_limits::quiet_NaN(), // Yaw unchanged coord.latitude(), diff --git a/src/MissionManager/SurveyMissionItem.h b/src/MissionManager/SurveyMissionItem.h index aa1d73dac..cdf4d6acb 100644 --- a/src/MissionManager/SurveyMissionItem.h +++ b/src/MissionManager/SurveyMissionItem.h @@ -95,6 +95,7 @@ public: // Overrides from ComplexMissionItem double complexDistance (void) const final { return _surveyDistance; } + double additionalTimeDelay (void) const final { return _additionalFlightDelaySeconds; } int lastSequenceNumber (void) const final; bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final; double greatestDistanceTo (const QGeoCoordinate &other) const final; @@ -231,6 +232,7 @@ private: bool _cameraOrientationFixed; int _missionCommandCount; bool _refly90Degrees; + double _additionalFlightDelaySeconds; bool _ignoreRecalc; double _surveyDistance; @@ -287,6 +289,8 @@ private: static const char* _jsonCameraOrientationLandscapeKey; static const char* _jsonFixedValueIsAltitudeKey; static const char* _jsonRefly90DegreesKey; + + static const int _hoverAndCaptureDelaySeconds = 1; }; #endif -- 2.22.0