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

Flight time calc for hover and capture

QGC4.4
DonLakeFlyer 8 years ago
parent
ee51717860
commit
1d3e7cae0a
5 changed files with 76 additions and 44 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. 6
      src/MissionManager/ComplexMissionItem.h
  2. 98
      src/MissionManager/MissionController.cc
  3. 3
      src/MissionManager/MissionController.h
  4. 9
      src/MissionManager/SurveyMissionItem.cc
  5. 4
      src/MissionManager/SurveyMissionItem.h

6
src/MissionManager/ComplexMissionItem.h
Unescape View File

@ -26,6 +26,9 @@ public: @@ -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: @@ -42,7 +45,8 @@ public:
static const char* jsonComplexItemTypeKey;
signals:
void complexDistanceChanged(double complexDistance);
void complexDistanceChanged (double complexDistance);
void additionalTimeDelayChanged (double additionalTimeDelay);
};
#endif

98
src/MissionManager/MissionController.cc
Unescape View File

@ -1017,6 +1017,53 @@ void MissionController::_addCruiseTime(double cruiseTime, double cruiseDistance, @@ -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() @@ -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() @@ -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() @@ -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() @@ -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) @@ -1416,7 +1429,8 @@ void MissionController::_initVisualItem(VisualMissionItem* visualItem)
} else {
ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(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";
}

3
src/MissionManager/MissionController.h
Unescape View File

@ -25,6 +25,7 @@ class MissionItem; @@ -25,6 +25,7 @@ class MissionItem;
class MissionSettingsItem;
class AppSettings;
class MissionManager;
class SimpleMissionItem;
Q_DECLARE_LOGGING_CATEGORY(MissionControllerLog)
@ -210,6 +211,8 @@ private: @@ -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;

9
src/MissionManager/SurveyMissionItem.cc
Unescape View File

@ -78,6 +78,7 @@ SurveyMissionItem::SurveyMissionItem(Vehicle* vehicle, QObject* parent) @@ -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) @@ -675,6 +676,7 @@ void SurveyMissionItem::_generateGrid(void)
_simpleGridPoints.clear();
_transectSegments.clear();
_reflyTransectSegments.clear();
_additionalFlightDelaySeconds = 0;
QList<QPointF> polygonPoints;
QList<QList<QPointF>> transectSegments;
@ -737,6 +739,11 @@ void SurveyMissionItem::_generateGrid(void) @@ -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<MissionItem*>& items, int @@ -1108,7 +1115,7 @@ int SurveyMissionItem::_appendWaypointToMission(QList<MissionItem*>& 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<double>::quiet_NaN(), // Yaw unchanged
coord.latitude(),

4
src/MissionManager/SurveyMissionItem.h
Unescape View File

@ -95,6 +95,7 @@ public: @@ -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: @@ -231,6 +232,7 @@ private:
bool _cameraOrientationFixed;
int _missionCommandCount;
bool _refly90Degrees;
double _additionalFlightDelaySeconds;
bool _ignoreRecalc;
double _surveyDistance;
@ -287,6 +289,8 @@ private: @@ -287,6 +289,8 @@ private:
static const char* _jsonCameraOrientationLandscapeKey;
static const char* _jsonFixedValueIsAltitudeKey;
static const char* _jsonRefly90DegreesKey;
static const int _hoverAndCaptureDelaySeconds = 1;
};
#endif

Write Preview
Loading…
Cancel
Save