/**************************************************************************** * * (c) 2009-2016 QGROUNDCONTROL PROJECT * * QGroundControl is licensed according to the terms in the file * COPYING.md in the root of the source code directory. * ****************************************************************************/ #include "MissionController.h" #include "MultiVehicleManager.h" #include "MissionManager.h" #include "CoordinateVector.h" #include "FirmwarePlugin.h" #include "QGCApplication.h" #include "SimpleMissionItem.h" #include "SurveyMissionItem.h" #include "FixedWingLandingComplexItem.h" #include "JsonHelper.h" #include "ParameterManager.h" #include "QGroundControlQmlGlobal.h" #include "SettingsManager.h" #include "MissionSettingsComplexItem.h" #ifndef __mobile__ #include "MainWindow.h" #include "QGCFileDialog.h" #endif QGC_LOGGING_CATEGORY(MissionControllerLog, "MissionControllerLog") const char* MissionController::_settingsGroup = "MissionController"; const char* MissionController::_jsonFileTypeValue = "Mission"; const char* MissionController::_jsonItemsKey = "items"; const char* MissionController::_jsonPlannedHomePositionKey = "plannedHomePosition"; const char* MissionController::_jsonFirmwareTypeKey = "firmwareType"; const char* MissionController::_jsonVehicleTypeKey = "vehicleType"; const char* MissionController::_jsonCruiseSpeedKey = "cruiseSpeed"; const char* MissionController::_jsonHoverSpeedKey = "hoverSpeed"; const char* MissionController::_jsonParamsKey = "params"; // Deprecated V1 format keys const char* MissionController::_jsonComplexItemsKey = "complexItems"; const char* MissionController::_jsonMavAutopilotKey = "MAV_AUTOPILOT"; const int MissionController::_missionFileVersion = 2; MissionController::MissionController(QObject *parent) : PlanElementController(parent) , _visualItems(NULL) , _firstItemsFromVehicle(false) , _missionItemsRequested(false) , _queuedSend(false) , _missionDistance(0.0) , _missionTime(0.0) , _missionHoverDistance(0.0) , _missionHoverTime(0.0) , _missionCruiseDistance(0.0) , _missionCruiseTime(0.0) , _missionMaxTelemetry(0.0) { _surveyMissionItemName = tr("Survey"); _fwLandingMissionItemName = tr("Fixed Wing Landing"); _complexMissionItemNames << _surveyMissionItemName << _fwLandingMissionItemName; } MissionController::~MissionController() { } void MissionController::start(bool editMode) { qCDebug(MissionControllerLog) << "start editMode" << editMode; PlanElementController::start(editMode); _init(); } void MissionController::startStaticActiveVehicle(Vehicle *vehicle) { qCDebug(MissionControllerLog) << "startStaticActiveVehicle"; PlanElementController::startStaticActiveVehicle(vehicle); _init(); } void MissionController::_init(void) { // We start with an empty mission _visualItems = new QmlObjectListModel(this); _addMissionSettings(_activeVehicle, _visualItems, false /* addToCenter */); _initAllVisualItems(); } // Called when new mission items have completed downloading from Vehicle void MissionController::_newMissionItemsAvailableFromVehicle(void) { qCDebug(MissionControllerLog) << "_newMissionItemsAvailableFromVehicle"; if (!_editMode || _missionItemsRequested || _visualItems->count() == 1) { // Fly Mode: // - Always accepts new items from the vehicle so Fly view is kept up to date // Edit Mode: // - Either a load from vehicle was manually requested or // - The initial automatic load from a vehicle completed and the current editor is empty QmlObjectListModel* newControllerMissionItems = new QmlObjectListModel(this); const QList& newMissionItems = _activeVehicle->missionManager()->missionItems(); qCDebug(MissionControllerLog) << "loading from vehicle: count"<< _visualItems->count(); foreach(const MissionItem* missionItem, newMissionItems) { newControllerMissionItems->append(new SimpleMissionItem(_activeVehicle, *missionItem, this)); } _deinitAllVisualItems(); _visualItems->deleteLater(); _visualItems = newControllerMissionItems; if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle() || _visualItems->count() == 0) { _addMissionSettings(_activeVehicle, _visualItems, true /* addToCenter */); } _missionItemsRequested = false; MissionSettingsComplexItem::scanForMissionSettings(_visualItems, _activeVehicle); _initAllVisualItems(); emit newItemsFromVehicle(); } } void MissionController::loadFromVehicle(void) { Vehicle* activeVehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle(); if (activeVehicle) { _missionItemsRequested = true; activeVehicle->missionManager()->requestMissionItems(); } } void MissionController::sendToVehicle(void) { sendItemsToVehicle(_activeVehicle, _visualItems); _visualItems->setDirty(false); } void MissionController::sendItemsToVehicle(Vehicle* vehicle, QmlObjectListModel* visualMissionItems) { if (vehicle) { // Convert to MissionItems so we can send to vehicle QList missionItems; for (int i=0; icount(); i++) { VisualMissionItem* visualItem = qobject_cast(visualMissionItems->get(i)); if (visualItem->isSimpleItem()) { missionItems.append(new MissionItem(qobject_cast(visualItem)->missionItem())); } else { ComplexMissionItem* complexItem = qobject_cast(visualItem); QmlObjectListModel* complexMissionItems = complexItem->getMissionItems(); for (int j=0; jcount(); j++) { missionItems.append(new MissionItem(*qobject_cast(complexMissionItems->get(j)))); } complexMissionItems->deleteLater(); } } vehicle->missionManager()->writeMissionItems(missionItems); for (int i=0; ideleteLater(); } } } int MissionController::_nextSequenceNumber(void) { if (_visualItems->count() == 0) { qWarning() << "Internal error: Empty visual item list"; return 0; } else { VisualMissionItem* lastItem = qobject_cast(_visualItems->get(_visualItems->count() - 1)); if (lastItem->isSimpleItem()) { return lastItem->sequenceNumber() + 1; } else { return qobject_cast(lastItem)->lastSequenceNumber() + 1; } } } int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i) { int sequenceNumber = _nextSequenceNumber(); SimpleMissionItem * newItem = new SimpleMissionItem(_activeVehicle, this); newItem->setSequenceNumber(sequenceNumber); newItem->setCoordinate(coordinate); newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT); _initVisualItem(newItem); if (_visualItems->count() == 1) { newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF); } newItem->setDefaultsForCommand(); if ((MAV_CMD)newItem->command() == MAV_CMD_NAV_WAYPOINT) { double prevAltitude; MAV_FRAME prevFrame; if (_findPreviousAltitude(i, &prevAltitude, &prevFrame)) { newItem->missionItem().setFrame(prevFrame); newItem->missionItem().setParam7(prevAltitude); } } _visualItems->insert(i, newItem); _recalcAll(); return newItem->sequenceNumber(); } int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate mapCenterCoordinate, int i) { ComplexMissionItem* newItem; int sequenceNumber = _nextSequenceNumber(); if (itemName == _surveyMissionItemName) { newItem = new SurveyMissionItem(_activeVehicle, _visualItems); newItem->setCoordinate(mapCenterCoordinate); } else if (itemName == _fwLandingMissionItemName) { newItem = new FixedWingLandingComplexItem(_activeVehicle, _visualItems); } else { qWarning() << "Internal error: Unknown complex item:" << itemName; return sequenceNumber; } newItem->setSequenceNumber(sequenceNumber); _initVisualItem(newItem); _visualItems->insert(i, newItem); _recalcAll(); return newItem->sequenceNumber(); } void MissionController::removeMissionItem(int index) { VisualMissionItem* item = qobject_cast(_visualItems->removeAt(index)); _deinitVisualItem(item); item->deleteLater(); _recalcAll(); _visualItems->setDirty(true); } void MissionController::removeAll(void) { if (_visualItems) { _deinitAllVisualItems(); _visualItems->deleteLater(); _visualItems = new QmlObjectListModel(this); _addMissionSettings(_activeVehicle, _visualItems, false /* addToCenter */); _initAllVisualItems(); _visualItems->setDirty(true); } } bool MissionController::_loadJsonMissionFile(Vehicle* vehicle, const QByteArray& bytes, QmlObjectListModel* visualItems, QString& errorString) { QJsonParseError jsonParseError; QJsonDocument jsonDoc(QJsonDocument::fromJson(bytes, &jsonParseError)); if (jsonParseError.error != QJsonParseError::NoError) { errorString = jsonParseError.errorString(); return false; } QJsonObject json = jsonDoc.object(); // V1 file format has no file type key and version key is string. Convert to new format. if (!json.contains(JsonHelper::jsonFileTypeKey)) { json[JsonHelper::jsonFileTypeKey] = _jsonFileTypeValue; } int fileVersion; if (!JsonHelper::validateQGCJsonFile(json, _jsonFileTypeValue, // expected file type 1, // minimum supported version 2, // maximum supported version fileVersion, errorString)) { return false; } if (fileVersion == 1) { return _loadJsonMissionFileV1(vehicle, json, visualItems, errorString); } else { return _loadJsonMissionFileV2(vehicle, json, visualItems, errorString); } } bool MissionController::_loadJsonMissionFileV1(Vehicle* vehicle, const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString) { // Validate root object keys QList rootKeyInfoList = { { _jsonPlannedHomePositionKey, QJsonValue::Object, true }, { _jsonItemsKey, QJsonValue::Array, true }, { _jsonMavAutopilotKey, QJsonValue::Double, true }, { _jsonComplexItemsKey, QJsonValue::Array, true }, }; if (!JsonHelper::validateKeys(json, rootKeyInfoList, errorString)) { return false; } // Read complex items QList surveyItems; QJsonArray complexArray(json[_jsonComplexItemsKey].toArray()); qCDebug(MissionControllerLog) << "Json load: complex item count" << complexArray.count(); for (int i=0; iload(itemObject, itemObject["id"].toInt(), errorString)) { surveyItems.append(item); } else { return false; } } // Read simple items, interspersing complex items into the full list int nextSimpleItemIndex= 0; int nextComplexItemIndex= 0; int nextSequenceNumber = 1; // Start with 1 since home is in 0 QJsonArray itemArray(json[_jsonItemsKey].toArray()); qCDebug(MissionControllerLog) << "Json load: simple item loop start simpleItemCount:ComplexItemCount" << itemArray.count() << surveyItems.count(); do { qCDebug(MissionControllerLog) << "Json load: simple item loop nextSimpleItemIndex:nextComplexItemIndex:nextSequenceNumber" << nextSimpleItemIndex << nextComplexItemIndex << nextSequenceNumber; // If there is a complex item that should be next in sequence add it in if (nextComplexItemIndex < surveyItems.count()) { SurveyMissionItem* complexItem = surveyItems[nextComplexItemIndex]; if (complexItem->sequenceNumber() == nextSequenceNumber) { qCDebug(MissionControllerLog) << "Json load: injecting complex item expectedSequence:actualSequence:" << nextSequenceNumber << complexItem->sequenceNumber(); visualItems->append(complexItem); nextSequenceNumber = complexItem->lastSequenceNumber() + 1; nextComplexItemIndex++; continue; } } // Add the next available simple item if (nextSimpleItemIndex < itemArray.count()) { const QJsonValue& itemValue = itemArray[nextSimpleItemIndex++]; if (!itemValue.isObject()) { errorString = QStringLiteral("Mission item is not an object"); return false; } const QJsonObject itemObject = itemValue.toObject(); SimpleMissionItem* item = new SimpleMissionItem(vehicle, visualItems); if (item->load(itemObject, itemObject["id"].toInt(), errorString)) { qCDebug(MissionControllerLog) << "Json load: adding simple item expectedSequence:actualSequence" << nextSequenceNumber << item->sequenceNumber(); visualItems->append(item); } else { return false; } nextSequenceNumber++; } } while (nextSimpleItemIndex < itemArray.count() || nextComplexItemIndex < surveyItems.count()); if (json.contains(_jsonPlannedHomePositionKey)) { SimpleMissionItem* item = new SimpleMissionItem(vehicle, visualItems); if (item->load(json[_jsonPlannedHomePositionKey].toObject(), 0, errorString)) { MissionSettingsComplexItem* settingsItem = new MissionSettingsComplexItem(vehicle, visualItems); settingsItem->setCoordinate(item->coordinate()); visualItems->insert(0, settingsItem); item->deleteLater(); } else { return false; } } else { _addMissionSettings(vehicle, visualItems, true /* addToCenter */); } return true; } bool MissionController::_loadJsonMissionFileV2(Vehicle* vehicle, const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString) { // Validate root object keys QList rootKeyInfoList = { { _jsonPlannedHomePositionKey, QJsonValue::Array, true }, { _jsonItemsKey, QJsonValue::Array, true }, { _jsonFirmwareTypeKey, QJsonValue::Double, true }, { _jsonVehicleTypeKey, QJsonValue::Double, false }, { _jsonCruiseSpeedKey, QJsonValue::Double, false }, { _jsonHoverSpeedKey, QJsonValue::Double, false }, }; if (!JsonHelper::validateKeys(json, rootKeyInfoList, errorString)) { return false; } qCDebug(MissionControllerLog) << "MissionController::_loadJsonMissionFileV2 itemCount:" << json[_jsonItemsKey].toArray().count(); // Mission Settings QGeoCoordinate homeCoordinate; SettingsManager* settingsManager = qgcApp()->toolbox()->settingsManager(); if (!JsonHelper::loadGeoCoordinate(json[_jsonPlannedHomePositionKey], true /* altitudeRequired */, homeCoordinate, errorString)) { return false; } if (json.contains(_jsonVehicleTypeKey) && vehicle->isOfflineEditingVehicle()) { settingsManager->appSettings()->offlineEditingVehicleType()->setRawValue(json[_jsonVehicleTypeKey].toDouble()); } if (json.contains(_jsonCruiseSpeedKey)) { settingsManager->appSettings()->offlineEditingCruiseSpeed()->setRawValue(json[_jsonCruiseSpeedKey].toDouble()); } if (json.contains(_jsonHoverSpeedKey)) { settingsManager->appSettings()->offlineEditingHoverSpeed()->setRawValue(json[_jsonHoverSpeedKey].toDouble()); } MissionSettingsComplexItem* settingsItem = new MissionSettingsComplexItem(vehicle, visualItems); settingsItem->setCoordinate(homeCoordinate); visualItems->insert(0, settingsItem); qCDebug(MissionControllerLog) << "plannedHomePosition" << homeCoordinate; // Read mission items int nextSequenceNumber = 1; // Start with 1 since home is in 0 const QJsonArray rgMissionItems(json[_jsonItemsKey].toArray()); for (int i=0; i itemKeyInfoList = { { VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, }; if (!JsonHelper::validateKeys(itemObject, itemKeyInfoList, errorString)) { return false; } QString itemType = itemObject[VisualMissionItem::jsonTypeKey].toString(); if (itemType == VisualMissionItem::jsonTypeSimpleItemValue) { qCDebug(MissionControllerLog) << "Loading MISSION_ITEM: nextSequenceNumber" << nextSequenceNumber; SimpleMissionItem* simpleItem = new SimpleMissionItem(vehicle, visualItems); if (simpleItem->load(itemObject, nextSequenceNumber++, errorString)) { visualItems->append(simpleItem); } else { return false; } } else if (itemType == VisualMissionItem::jsonTypeComplexItemValue) { QList complexItemKeyInfoList = { { ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true }, }; if (!JsonHelper::validateKeys(itemObject, complexItemKeyInfoList, errorString)) { return false; } QString complexItemType = itemObject[ComplexMissionItem::jsonComplexItemTypeKey].toString(); if (complexItemType == SurveyMissionItem::jsonComplexItemTypeValue) { qCDebug(MissionControllerLog) << "Loading Survey: nextSequenceNumber" << nextSequenceNumber; SurveyMissionItem* surveyItem = new SurveyMissionItem(vehicle, visualItems); if (!surveyItem->load(itemObject, nextSequenceNumber++, errorString)) { return false; } nextSequenceNumber = surveyItem->lastSequenceNumber() + 1; qCDebug(MissionControllerLog) << "Survey load complete: nextSequenceNumber" << nextSequenceNumber; visualItems->append(surveyItem); } else if (complexItemType == FixedWingLandingComplexItem::jsonComplexItemTypeValue) { qCDebug(MissionControllerLog) << "Loading Fixed Wing Landing Pattern: nextSequenceNumber" << nextSequenceNumber; FixedWingLandingComplexItem* landingItem = new FixedWingLandingComplexItem(vehicle, visualItems); if (!landingItem->load(itemObject, nextSequenceNumber++, errorString)) { return false; } nextSequenceNumber = landingItem->lastSequenceNumber() + 1; qCDebug(MissionControllerLog) << "FW Landing Pattern load complete: nextSequenceNumber" << nextSequenceNumber; visualItems->append(landingItem); } else if (complexItemType == MissionSettingsComplexItem::jsonComplexItemTypeValue) { qCDebug(MissionControllerLog) << "Loading Mission Settings: nextSequenceNumber" << nextSequenceNumber; MissionSettingsComplexItem* settingsItem = new MissionSettingsComplexItem(vehicle, visualItems); if (!settingsItem->load(itemObject, nextSequenceNumber++, errorString)) { return false; } nextSequenceNumber = settingsItem->lastSequenceNumber() + 1; qCDebug(MissionControllerLog) << "Mission Settings load complete: nextSequenceNumber" << nextSequenceNumber; visualItems->append(settingsItem); } else { errorString = tr("Unsupported complex item type: %1").arg(complexItemType); } } else { errorString = tr("Unknown item type: %1").arg(itemType); return false; } } // Fix up the DO_JUMP commands jump sequence number by finding the item with the matching doJumpId for (int i=0; icount(); i++) { if (visualItems->value(i)->isSimpleItem()) { SimpleMissionItem* doJumpItem = visualItems->value(i); if ((MAV_CMD)doJumpItem->command() == MAV_CMD_DO_JUMP) { bool found = false; int findDoJumpId = doJumpItem->missionItem().param1(); for (int j=0; jcount(); j++) { if (visualItems->value(j)->isSimpleItem()) { SimpleMissionItem* targetItem = visualItems->value(j); if (targetItem->missionItem().doJumpId() == findDoJumpId) { doJumpItem->missionItem().setParam1(targetItem->sequenceNumber()); found = true; break; } } } if (!found) { errorString = tr("Could not find doJumpId: %1").arg(findDoJumpId); return false; } } } } return true; } bool MissionController::_loadTextMissionFile(Vehicle* vehicle, QTextStream& stream, QmlObjectListModel* visualItems, QString& errorString) { bool addPlannedHomePosition = false; QString firstLine = stream.readLine(); const QStringList& version = firstLine.split(" "); bool versionOk = false; if (version.size() == 3 && version[0] == "QGC" && version[1] == "WPL") { if (version[2] == "110") { // ArduPilot file, planned home position is already in position 0 versionOk = true; } else if (version[2] == "120") { // Old QGC file, no planned home position versionOk = true; addPlannedHomePosition = true; } } if (versionOk) { while (!stream.atEnd()) { SimpleMissionItem* item = new SimpleMissionItem(vehicle, visualItems); if (item->load(stream)) { visualItems->append(item); } else { errorString = QStringLiteral("The mission file is corrupted."); return false; } } } else { errorString = QStringLiteral("The mission file is not compatible with this version of QGroundControl."); return false; } if (addPlannedHomePosition || visualItems->count() == 0) { _addMissionSettings(vehicle, visualItems, true /* addToCenter */); // Update sequence numbers in DO_JUMP commands to take into account added home position in index 0 for (int i=1; icount(); i++) { SimpleMissionItem* item = qobject_cast(visualItems->get(i)); if (item && item->command() == MavlinkQmlSingleton::MAV_CMD_DO_JUMP) { item->missionItem().setParam1((int)item->missionItem().param1() + 1); } } } return true; } void MissionController::loadFromFile(const QString& filename) { QmlObjectListModel* newVisualItems = NULL; if (!loadItemsFromFile(_activeVehicle, filename, &newVisualItems)) { return; } if (_visualItems) { _deinitAllVisualItems(); _visualItems->deleteLater(); } _visualItems = newVisualItems; if (_visualItems->count() == 0) { _addMissionSettings(_activeVehicle, _visualItems, true /* addToCenter */); } MissionSettingsComplexItem::scanForMissionSettings(_visualItems, _activeVehicle); _initAllVisualItems(); } bool MissionController::loadItemsFromFile(Vehicle* vehicle, const QString& filename, QmlObjectListModel** visualItems) { *visualItems = NULL; QString errorString; if (filename.isEmpty()) { return false; } *visualItems = new QmlObjectListModel(); QFile file(filename); if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { errorString = file.errorString() + QStringLiteral(" ") + filename; } else { QByteArray bytes = file.readAll(); QTextStream stream(&bytes); QString firstLine = stream.readLine(); if (firstLine.contains(QRegExp("QGC.*WPL"))) { stream.seek(0); _loadTextMissionFile(vehicle, stream, *visualItems, errorString); } else { _loadJsonMissionFile(vehicle, bytes, *visualItems, errorString); } } if (!errorString.isEmpty()) { (*visualItems)->deleteLater(); qgcApp()->showMessage(errorString); return false; } return true; } void MissionController::loadFromFilePicker(void) { #ifndef __mobile__ QString filename = QGCFileDialog::getOpenFileName(MainWindow::instance(), "Select Mission File to load", QString(), "Mission file (*.mission);;All Files (*.*)"); if (filename.isEmpty()) { return; } loadFromFile(filename); #endif } void MissionController::saveToFile(const QString& filename) { qDebug() << filename; if (filename.isEmpty()) { return; } QString missionFilename = filename; if (!QFileInfo(filename).fileName().contains(".")) { missionFilename += QString(".%1").arg(QGCApplication::missionFileExtension); } QFile file(missionFilename); if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) { qgcApp()->showMessage(file.errorString()); } else { QJsonObject missionFileObject; // top level json object missionFileObject[JsonHelper::jsonVersionKey] = _missionFileVersion; missionFileObject[JsonHelper::jsonGroundStationKey] = JsonHelper::jsonGroundStationValue; // Mission settings MissionSettingsComplexItem* settingsItem = _visualItems->value(0); if (!settingsItem) { qWarning() << "First item is not MissionSettingsComplexItem"; return; } QJsonValue coordinateValue; JsonHelper::saveGeoCoordinate(settingsItem->coordinate(), true /* writeAltitude */, coordinateValue); missionFileObject[_jsonPlannedHomePositionKey] = coordinateValue; missionFileObject[_jsonFirmwareTypeKey] = _activeVehicle->firmwareType(); missionFileObject[_jsonVehicleTypeKey] = _activeVehicle->vehicleType(); missionFileObject[_jsonCruiseSpeedKey] = _activeVehicle->cruiseSpeed(); missionFileObject[_jsonHoverSpeedKey] = _activeVehicle->hoverSpeed(); // Save the visual items QJsonArray rgMissionItems; for (int i=0; i<_visualItems->count(); i++) { VisualMissionItem* visualItem = qobject_cast(_visualItems->get(i)); visualItem->save(rgMissionItems); } missionFileObject[_jsonItemsKey] = rgMissionItems; QJsonDocument saveDoc(missionFileObject); file.write(saveDoc.toJson()); } _visualItems->setDirty(false); } void MissionController::saveToFilePicker(void) { #ifndef __mobile__ QString filename = QGCFileDialog::getSaveFileName(MainWindow::instance(), "Select file to save mission to", QString(), "Mission file (*.mission);;All Files (*.*)"); if (filename.isEmpty()) { return; } saveToFile(filename); #endif } void MissionController::_calcPrevWaypointValues(double homeAlt, VisualMissionItem* currentItem, VisualMissionItem* prevItem, double* azimuth, double* distance, double* altDifference) { QGeoCoordinate currentCoord = currentItem->coordinate(); QGeoCoordinate prevCoord = prevItem->exitCoordinate(); bool distanceOk = false; // Convert to fixed altitudes qCDebug(MissionControllerLog) << homeAlt << currentItem->coordinateHasRelativeAltitude() << currentItem->coordinate().altitude() << prevItem->exitCoordinateHasRelativeAltitude() << prevItem->exitCoordinate().altitude(); distanceOk = true; if (currentItem->coordinateHasRelativeAltitude()) { currentCoord.setAltitude(homeAlt + currentCoord.altitude()); } if (prevItem->exitCoordinateHasRelativeAltitude()) { prevCoord.setAltitude(homeAlt + prevCoord.altitude()); } qCDebug(MissionControllerLog) << "distanceOk" << distanceOk; if (distanceOk) { *altDifference = currentCoord.altitude() - prevCoord.altitude(); *distance = prevCoord.distanceTo(currentCoord); *azimuth = prevCoord.azimuthTo(currentCoord); } else { *altDifference = 0.0; *azimuth = 0.0; *distance = 0.0; } } double MissionController::_calcDistanceToHome(VisualMissionItem* currentItem, VisualMissionItem* homeItem) { QGeoCoordinate currentCoord = currentItem->coordinate(); QGeoCoordinate homeCoord = homeItem->exitCoordinate(); bool distanceOk = false; distanceOk = true; qCDebug(MissionControllerLog) << "distanceOk" << distanceOk; return distanceOk ? homeCoord.distanceTo(currentCoord) : 0.0; } void MissionController::_recalcWaypointLines(void) { bool firstCoordinateItem = true; VisualMissionItem* lastCoordinateItem = qobject_cast(_visualItems->get(0)); MissionSettingsComplexItem* settingsItem = qobject_cast(lastCoordinateItem); if (!settingsItem) { qWarning() << "First item is not MissionSettingsComplexItem"; } bool showHomePosition = false; // FIXME: settingsItem->showHomePosition(); qCDebug(MissionControllerLog) << "_recalcWaypointLines"; CoordVectHashTable old_table = _linesTable; _linesTable.clear(); _waypointLines.clear(); bool linkBackToHome = false; for (int i=1; i<_visualItems->count(); i++) { VisualMissionItem* item = qobject_cast(_visualItems->get(i)); // If we still haven't found the first coordinate item and we hit a takeoff command, link back to home if (firstCoordinateItem && item->isSimpleItem() && (qobject_cast(item)->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF || qobject_cast(item)->command() == MavlinkQmlSingleton::MAV_CMD_NAV_VTOL_TAKEOFF)) { linkBackToHome = true; } if (item->specifiesCoordinate()) { if (!item->isStandaloneCoordinate()) { firstCoordinateItem = false; VisualItemPair pair(lastCoordinateItem, item); if (lastCoordinateItem != settingsItem || (showHomePosition && linkBackToHome)) { if (old_table.contains(pair)) { // Do nothing, this segment already exists and is wired up _linesTable[pair] = old_table.take(pair); } else { // Create a new segment and wire update notifiers auto linevect = new CoordinateVector(lastCoordinateItem->isSimpleItem() ? lastCoordinateItem->coordinate() : lastCoordinateItem->exitCoordinate(), item->coordinate(), this); auto originNotifier = lastCoordinateItem->isSimpleItem() ? &VisualMissionItem::coordinateChanged : &VisualMissionItem::exitCoordinateChanged, endNotifier = &VisualMissionItem::coordinateChanged; // Use signals/slots to update the coordinate endpoints connect(lastCoordinateItem, originNotifier, linevect, &CoordinateVector::setCoordinate1); connect(item, endNotifier, linevect, &CoordinateVector::setCoordinate2); // FIXME: We should ideally have signals for 2D position change, alt change, and 3D position change // Not optimal, but still pretty fast, do a full update of range/bearing/altitudes connect(item, &VisualMissionItem::coordinateChanged, this, &MissionController::_recalcAltitudeRangeBearing); _linesTable[pair] = linevect; } } lastCoordinateItem = item; } } } { // Create a temporary QObjectList and replace the model data QObjectList objs; objs.reserve(_linesTable.count()); foreach(CoordinateVector *vect, _linesTable.values()) { objs.append(vect); } // We don't delete here because many links may still be valid _waypointLines.swapObjectList(objs); } // Anything left in the old table is an obsolete line object that can go qDeleteAll(old_table); _recalcAltitudeRangeBearing(); emit waypointLinesChanged(); } void MissionController::_recalcAltitudeRangeBearing() { if (!_visualItems->count()) { return; } bool firstCoordinateItem = true; VisualMissionItem* lastCoordinateItem = qobject_cast(_visualItems->get(0)); MissionSettingsComplexItem* settingsItem = qobject_cast(lastCoordinateItem); if (!settingsItem) { qWarning() << "First item is not MissionSettingsComplexItem"; } bool showHomePosition = settingsItem->showHomePosition(); qCDebug(MissionControllerLog) << "_recalcAltitudeRangeBearing"; // If home position is valid we can calculate distances between all waypoints. // If home position is not valid we can only calculate distances between waypoints which are // both relative altitude. // No values for first item lastCoordinateItem->setAltDifference(0.0); lastCoordinateItem->setAzimuth(0.0); lastCoordinateItem->setDistance(0.0); double minAltSeen = 0.0; double maxAltSeen = 0.0; const double homePositionAltitude = settingsItem->coordinate().altitude(); minAltSeen = maxAltSeen = settingsItem->coordinate().altitude(); double missionDistance = 0.0; double missionMaxTelemetry = 0.0; double missionTime = 0.0; double vtolHoverTime = 0.0; double vtolCruiseTime = 0.0; double vtolHoverDistance = 0.0; double vtolCruiseDistance = 0.0; double currentCruiseSpeed = _activeVehicle->cruiseSpeed(); double currentHoverSpeed = _activeVehicle->hoverSpeed(); bool vtolVehicle = _activeVehicle->vtol(); bool vtolInHover = true; bool linkBackToHome = false; for (int i=1; i<_visualItems->count(); i++) { VisualMissionItem* item = qobject_cast(_visualItems->get(i)); SimpleMissionItem* simpleItem = qobject_cast(item); ComplexMissionItem* complexItem = qobject_cast(item); // Assume the worst item->setAzimuth(0.0); item->setDistance(0.0); if (simpleItem && simpleItem->command() == MavlinkQmlSingleton::MAV_CMD_DO_CHANGE_SPEED) { // Adjust cruise speed for time calculations double newSpeed = simpleItem->missionItem().param2(); if (newSpeed > 0) { if (_activeVehicle->multiRotor()) { currentHoverSpeed = newSpeed; } else { currentCruiseSpeed = newSpeed; } } } // Link back to home if first item is takeoff and we have home position if (firstCoordinateItem && simpleItem && simpleItem->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) { if (showHomePosition) { linkBackToHome = true; } } // Update VTOL state if (simpleItem && vtolVehicle) { switch (simpleItem->command()) { case MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF: vtolInHover = false; break; case MavlinkQmlSingleton::MAV_CMD_NAV_LAND: vtolInHover = false; break; case MavlinkQmlSingleton::MAV_CMD_DO_VTOL_TRANSITION: { int transitionState = simpleItem->missionItem().param1(); if (transitionState == MAV_VTOL_STATE_TRANSITION_TO_MC) { vtolInHover = true; } else if (transitionState == MAV_VTOL_STATE_TRANSITION_TO_FW) { vtolInHover = false; } } break; default: break; } } if (item->specifiesCoordinate()) { // Keep track of the min/max altitude for all waypoints so we can show altitudes as a percentage double absoluteAltitude = item->coordinate().altitude(); if (item->coordinateHasRelativeAltitude()) { absoluteAltitude += homePositionAltitude; } minAltSeen = std::min(minAltSeen, absoluteAltitude); maxAltSeen = std::max(maxAltSeen, absoluteAltitude); if (!item->exitCoordinateSameAsEntry()) { absoluteAltitude = item->exitCoordinate().altitude(); if (item->exitCoordinateHasRelativeAltitude()) { absoluteAltitude += homePositionAltitude; } minAltSeen = std::min(minAltSeen, absoluteAltitude); maxAltSeen = std::max(maxAltSeen, absoluteAltitude); } if (!item->isStandaloneCoordinate()) { firstCoordinateItem = false; if (lastCoordinateItem != settingsItem || linkBackToHome) { // This is a subsequent waypoint or we are forcing the first waypoint back to home double azimuth, distance, altDifference; _calcPrevWaypointValues(homePositionAltitude, item, lastCoordinateItem, &azimuth, &distance, &altDifference); item->setAltDifference(altDifference); item->setAzimuth(azimuth); item->setDistance(distance); missionDistance += distance; missionMaxTelemetry = qMax(missionMaxTelemetry, _calcDistanceToHome(item, settingsItem)); // Calculate mission time if (vtolVehicle) { if (vtolInHover) { double hoverTime = distance / _activeVehicle->hoverSpeed(); missionTime += hoverTime; vtolHoverTime += hoverTime; vtolHoverDistance += distance; } else { double cruiseTime = distance / currentCruiseSpeed; missionTime += cruiseTime; vtolCruiseTime += cruiseTime; vtolCruiseDistance += distance; } } else { missionTime += distance / (_activeVehicle->multiRotor() ? currentHoverSpeed : currentCruiseSpeed); } } if (complexItem) { // Add in distance/time inside survey as well // This code assumes all surveys are done cruise not hover double complexDistance = complexItem->complexDistance(); double cruiseSpeed = _activeVehicle->multiRotor() ? currentHoverSpeed : currentCruiseSpeed; missionDistance += complexDistance; missionTime += complexDistance / cruiseSpeed; missionMaxTelemetry = qMax(missionMaxTelemetry, complexItem->greatestDistanceTo(settingsItem->exitCoordinate())); // Let the complex item know the current cruise speed complexItem->setCruiseSpeed(cruiseSpeed); } } lastCoordinateItem = item; } } _setMissionMaxTelemetry(missionMaxTelemetry); _setMissionDistance(missionDistance); _setMissionTime(missionTime); _setMissionHoverDistance(vtolHoverDistance); _setMissionHoverTime(vtolHoverTime); _setMissionCruiseDistance(vtolCruiseDistance); _setMissionCruiseTime(vtolCruiseTime); // Walk the list again calculating altitude percentages double altRange = maxAltSeen - minAltSeen; for (int i=0; i<_visualItems->count(); i++) { VisualMissionItem* item = qobject_cast(_visualItems->get(i)); if (item->specifiesCoordinate()) { double absoluteAltitude = item->coordinate().altitude(); if (item->coordinateHasRelativeAltitude()) { absoluteAltitude += homePositionAltitude; } if (altRange == 0.0) { item->setAltPercent(0.0); } else { item->setAltPercent((absoluteAltitude - minAltSeen) / altRange); } } } } // This will update the sequence numbers to be sequential starting from 0 void MissionController::_recalcSequence(void) { // Setup ascending sequence numbers for all visual items int sequenceNumber = 0; for (int i=0; i<_visualItems->count(); i++) { VisualMissionItem* item = qobject_cast(_visualItems->get(i)); item->setSequenceNumber(sequenceNumber++); if (!item->isSimpleItem()) { ComplexMissionItem* complexItem = qobject_cast(item); if (complexItem) { sequenceNumber = complexItem->lastSequenceNumber() + 1; } else { qWarning() << "isSimpleItem == false, yet not ComplexMissionItem"; } } } } // This will update the child item hierarchy void MissionController::_recalcChildItems(void) { VisualMissionItem* currentParentItem = qobject_cast(_visualItems->get(0)); currentParentItem->childItems()->clear(); for (int i=1; i<_visualItems->count(); i++) { VisualMissionItem* item = qobject_cast(_visualItems->get(i)); // Set up non-coordinate item child hierarchy if (item->specifiesCoordinate()) { item->childItems()->clear(); currentParentItem = item; } else if (item->isSimpleItem()) { currentParentItem->childItems()->append(item); } } } void MissionController::_recalcAll(void) { _recalcSequence(); _recalcChildItems(); _recalcWaypointLines(); } /// Initializes a new set of mission items void MissionController::_initAllVisualItems(void) { MissionSettingsComplexItem* settingsItem = NULL; // Setup home position at index 0 settingsItem = qobject_cast(_visualItems->get(0)); if (!settingsItem) { qWarning() << "First item not MissionSettingsComplexItem"; return; } settingsItem->setShowHomePosition(_editMode); settingsItem->setIsCurrentItem(true); if (!_editMode && _activeVehicle && _activeVehicle->homePositionAvailable()) { settingsItem->setCoordinate(_activeVehicle->homePosition()); settingsItem->setShowHomePosition(true); } emit plannedHomePositionChanged(plannedHomePosition()); connect(settingsItem, &VisualMissionItem::coordinateChanged, this, &MissionController::_homeCoordinateChanged); for (int i=0; i<_visualItems->count(); i++) { VisualMissionItem* item = qobject_cast(_visualItems->get(i)); _initVisualItem(item); } _recalcAll(); emit visualItemsChanged(); connect(_visualItems, &QmlObjectListModel::dirtyChanged, this, &MissionController::dirtyChanged); _visualItems->setDirty(false); } void MissionController::_deinitAllVisualItems(void) { for (int i=0; i<_visualItems->count(); i++) { _deinitVisualItem(qobject_cast(_visualItems->get(i))); } disconnect(_visualItems, &QmlObjectListModel::dirtyChanged, this, &MissionController::dirtyChanged); } void MissionController::_initVisualItem(VisualMissionItem* visualItem) { _visualItems->setDirty(false); connect(visualItem, &VisualMissionItem::specifiesCoordinateChanged, this, &MissionController::_recalcWaypointLines); connect(visualItem, &VisualMissionItem::coordinateHasRelativeAltitudeChanged, this, &MissionController::_recalcWaypointLines); connect(visualItem, &VisualMissionItem::exitCoordinateHasRelativeAltitudeChanged, this, &MissionController::_recalcWaypointLines); connect(visualItem, &VisualMissionItem::flightSpeedChanged, this, &MissionController::_recalcAltitudeRangeBearing); if (visualItem->isSimpleItem()) { // We need to track commandChanged on simple item since recalc has special handling for takeoff command SimpleMissionItem* simpleItem = qobject_cast(visualItem); if (simpleItem) { connect(&simpleItem->missionItem()._commandFact, &Fact::valueChanged, this, &MissionController::_itemCommandChanged); } else { qWarning() << "isSimpleItem == true, yet not SimpleMissionItem"; } } else { // We need to track changes of lastSequenceNumber so we can recalc sequence numbers for subsequence items ComplexMissionItem* complexItem = qobject_cast(visualItem); connect(complexItem, &ComplexMissionItem::lastSequenceNumberChanged, this, &MissionController::_recalcSequence); connect(complexItem, &ComplexMissionItem::complexDistanceChanged, this, &MissionController::_recalcAltitudeRangeBearing); } } void MissionController::_deinitVisualItem(VisualMissionItem* visualItem) { // Disconnect all signals disconnect(visualItem, 0, 0, 0); } void MissionController::_itemCommandChanged(void) { _recalcChildItems(); _recalcWaypointLines(); } void MissionController::_activeVehicleBeingRemoved(void) { qCDebug(MissionControllerLog) << "MissionController::_activeVehicleBeingRemoved"; MissionManager* missionManager = _activeVehicle->missionManager(); disconnect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionController::_newMissionItemsAvailableFromVehicle); disconnect(missionManager, &MissionManager::inProgressChanged, this, &MissionController::_inProgressChanged); disconnect(missionManager, &MissionManager::currentItemChanged, this, &MissionController::_currentMissionItemChanged); disconnect(_activeVehicle, &Vehicle::homePositionAvailableChanged, this, &MissionController::_activeVehicleHomePositionAvailableChanged); disconnect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionController::_activeVehicleHomePositionChanged); // We always remove all items on vehicle change. This leaves a user model hole: // If the user has unsaved changes in the Plan view they will lose them removeAll(); } void MissionController::_activeVehicleSet(void) { // We always remove all items on vehicle change. This leaves a user model hole: // If the user has unsaved changes in the Plan view they will lose them removeAll(); MissionManager* missionManager = _activeVehicle->missionManager(); connect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionController::_newMissionItemsAvailableFromVehicle); connect(missionManager, &MissionManager::inProgressChanged, this, &MissionController::_inProgressChanged); connect(missionManager, &MissionManager::currentItemChanged, this, &MissionController::_currentMissionItemChanged); connect(_activeVehicle, &Vehicle::homePositionAvailableChanged, this, &MissionController::_activeVehicleHomePositionAvailableChanged); connect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionController::_activeVehicleHomePositionChanged); connect(_activeVehicle, &Vehicle::cruiseSpeedChanged, this, &MissionController::_recalcAltitudeRangeBearing); connect(_activeVehicle, &Vehicle::hoverSpeedChanged, this, &MissionController::_recalcAltitudeRangeBearing); if (_activeVehicle->parameterManager()->parametersReady() && !syncInProgress()) { // We are switching between two previously existing vehicles. We have to manually ask for the items from the Vehicle. // We don't request mission items for new vehicles since that will happen autamatically. loadFromVehicle(); } _activeVehicleHomePositionChanged(_activeVehicle->homePosition()); _activeVehicleHomePositionAvailableChanged(_activeVehicle->homePositionAvailable()); } void MissionController::_activeVehicleHomePositionAvailableChanged(bool homePositionAvailable) { if (!_editMode && _visualItems) { MissionSettingsComplexItem* settingsItem = qobject_cast(_visualItems->get(0)); if (settingsItem) { settingsItem->setShowHomePosition(homePositionAvailable); emit plannedHomePositionChanged(plannedHomePosition()); _recalcWaypointLines(); } else { qWarning() << "First item is not MissionSettingsComplexItem"; } } } void MissionController::_activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition) { if (!_editMode && _visualItems) { MissionSettingsComplexItem* settingsItem = qobject_cast(_visualItems->get(0)); if (settingsItem) { if (settingsItem->coordinate() != homePosition) { settingsItem->setCoordinate(homePosition); settingsItem->setShowHomePosition(true); qCDebug(MissionControllerLog) << "Home position update" << homePosition; emit plannedHomePositionChanged(plannedHomePosition()); _recalcWaypointLines(); } } else { qWarning() << "First item is not MissionSettingsComplexItem"; } } } void MissionController::_setMissionMaxTelemetry(double missionMaxTelemetry) { if (!qFuzzyCompare(_missionMaxTelemetry, missionMaxTelemetry)) { _missionMaxTelemetry = missionMaxTelemetry; emit missionMaxTelemetryChanged(_missionMaxTelemetry); } } void MissionController::_setMissionDistance(double missionDistance) { if (!qFuzzyCompare(_missionDistance, missionDistance)) { _missionDistance = missionDistance; emit missionDistanceChanged(_missionDistance); } } void MissionController::_setMissionTime(double missionTime) { if (!qFuzzyCompare(_missionTime, missionTime)) { _missionTime = missionTime; emit missionTimeChanged(); } } void MissionController::_setMissionHoverTime(double missionHoverTime) { if (!qFuzzyCompare(_missionHoverTime, missionHoverTime)) { _missionHoverTime = missionHoverTime; emit missionHoverTimeChanged(); } } void MissionController::_setMissionHoverDistance(double missionHoverDistance) { if (!qFuzzyCompare(_missionHoverDistance, missionHoverDistance)) { _missionHoverDistance = missionHoverDistance; emit missionHoverDistanceChanged(_missionHoverDistance); } } void MissionController::_setMissionCruiseTime(double missionCruiseTime) { if (!qFuzzyCompare(_missionCruiseTime, missionCruiseTime)) { _missionCruiseTime = missionCruiseTime; emit missionCruiseTimeChanged(); } } void MissionController::_setMissionCruiseDistance(double missionCruiseDistance) { if (!qFuzzyCompare(_missionCruiseDistance, missionCruiseDistance)) { _missionCruiseDistance = missionCruiseDistance; emit missionCruiseDistanceChanged(_missionCruiseDistance); } } void MissionController::_inProgressChanged(bool inProgress) { emit syncInProgressChanged(inProgress); } bool MissionController::_findPreviousAltitude(int newIndex, double* prevAltitude, MAV_FRAME* prevFrame) { bool found = false; double foundAltitude; MAV_FRAME foundFrame; if (newIndex > _visualItems->count()) { return false; } newIndex--; for (int i=newIndex; i>0; i--) { VisualMissionItem* visualItem = qobject_cast(_visualItems->get(i)); if (visualItem->specifiesCoordinate() && !visualItem->isStandaloneCoordinate()) { if (visualItem->isSimpleItem()) { SimpleMissionItem* simpleItem = qobject_cast(visualItem); if ((MAV_CMD)simpleItem->command() == MAV_CMD_NAV_WAYPOINT) { foundAltitude = simpleItem->exitCoordinate().altitude(); foundFrame = simpleItem->missionItem().frame(); found = true; break; } } } } if (found) { *prevAltitude = foundAltitude; *prevFrame = foundFrame; } return found; } double MissionController::_normalizeLat(double lat) { // Normalize latitude to range: 0 to 180, S to N return lat + 90.0; } double MissionController::_normalizeLon(double lon) { // Normalize longitude to range: 0 to 360, W to E return lon + 180.0; } /// Add the Mission Settings complex item to the front of the items void MissionController::_addMissionSettings(Vehicle* vehicle, QmlObjectListModel* visualItems, bool addToCenter) { bool homePositionSet = false; MissionSettingsComplexItem* settingsItem = new MissionSettingsComplexItem(vehicle, visualItems); visualItems->insert(0, settingsItem); if (visualItems->count() > 1 && addToCenter) { double north = 0.0; double south = 0.0; double east = 0.0; double west = 0.0; bool firstCoordSet = false; for (int i=1; icount(); i++) { VisualMissionItem* item = qobject_cast(visualItems->get(i)); if (item->specifiesCoordinate()) { if (firstCoordSet) { double lat = _normalizeLat(item->coordinate().latitude()); double lon = _normalizeLon(item->coordinate().longitude()); north = fmax(north, lat); south = fmin(south, lat); east = fmax(east, lon); west = fmin(west, lon); } else { firstCoordSet = true; north = _normalizeLat(item->coordinate().latitude()); south = north; east = _normalizeLon(item->coordinate().longitude()); west = east; } } } if (firstCoordSet) { homePositionSet = true; settingsItem->setCoordinate(QGeoCoordinate((south + ((north - south) / 2)) - 90.0, (west + ((east - west) / 2)) - 180.0, 0.0)); } } if (!homePositionSet) { settingsItem->setCoordinate(qgcApp()->lastKnownHomePosition()); } } void MissionController::_currentMissionItemChanged(int sequenceNumber) { if (!_editMode) { if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle()) { sequenceNumber++; } for (int i=0; i<_visualItems->count(); i++) { VisualMissionItem* item = qobject_cast(_visualItems->get(i)); item->setIsCurrentItem(item->sequenceNumber() == sequenceNumber); } } } bool MissionController::syncInProgress(void) const { return _activeVehicle ? _activeVehicle->missionManager()->inProgress() : false; } bool MissionController::dirty(void) const { return _visualItems ? _visualItems->dirty() : false; } void MissionController::setDirty(bool dirty) { if (_visualItems) { _visualItems->setDirty(dirty); } } QGeoCoordinate MissionController::plannedHomePosition(void) { if (_visualItems && _visualItems->count() > 0) { MissionSettingsComplexItem* settingsItem = qobject_cast(_visualItems->get(0)); if (settingsItem && settingsItem->showHomePosition()) { return settingsItem->coordinate(); } } return QGeoCoordinate(); } void MissionController::_homeCoordinateChanged(void) { emit plannedHomePositionChanged(plannedHomePosition()); _recalcAltitudeRangeBearing(); } QString MissionController::fileExtension(void) const { return QGCApplication::missionFileExtension; } double MissionController::cruiseSpeed(void) const { if (_activeVehicle) { return _activeVehicle->cruiseSpeed(); } else { return 0.0f; } } double MissionController::hoverSpeed(void) const { if (_activeVehicle) { return _activeVehicle->hoverSpeed(); } else { return 0.0f; } }