#include "WimaController.h" const char* WimaController::wimaFileExtension = "wima"; const char* WimaController::areaItemsName = "AreaItems"; const char* WimaController::missionItemsName = "MissionItems"; const char* WimaController::settingsGroup = "WimaController"; const char* WimaController::enableWimaControllerName = "EnableWimaController"; const char* WimaController::overlapWaypointsName = "OverlapWaypoints"; const char* WimaController::maxWaypointsPerPhaseName = "MaxWaypointsPerPhase"; const char* WimaController::startWaypointIndexName = "StartWaypointIndex"; const char* WimaController::showAllMissionItemsName = "ShowAllMissionItems"; const char* WimaController::showCurrentMissionItemsName = "ShowCurrentMissionItems"; WimaController::WimaController(QObject *parent) : QObject (parent) , _container (nullptr) , _joinedArea (this) , _measurementArea (this) , _serviceArea (this) , _corridor (this) , _localPlanDataValid (false) , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/WimaController.SettingsGroup.json"), this)) , _enableWimaController (settingsGroup, _metaDataMap[enableWimaControllerName]) , _overlapWaypoints (settingsGroup, _metaDataMap[overlapWaypointsName]) , _maxWaypointsPerPhase (settingsGroup, _metaDataMap[maxWaypointsPerPhaseName]) , _startWaypointIndex (settingsGroup, _metaDataMap[startWaypointIndexName]) , _showAllMissionItems (settingsGroup, _metaDataMap[showAllMissionItemsName]) , _showCurrentMissionItems (settingsGroup, _metaDataMap[showCurrentMissionItemsName]) { _startWaypointIndex.setRawValue(int(1)); _showAllMissionItems.setRawValue(true); _showCurrentMissionItems.setRawValue(true); connect(&_overlapWaypoints, &Fact::rawValueChanged, this, &WimaController::updateNextWaypoint); connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this, &WimaController::updateCurrentMissionItems); connect(&_startWaypointIndex, &Fact::rawValueChanged, this, &WimaController::updateCurrentMissionItems); } QmlObjectListModel* WimaController::visualItems() { return &_visualItems; } QStringList WimaController::loadNameFilters() const { QStringList filters; filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension) << tr("All Files (*.*)"); return filters; } QStringList WimaController::saveNameFilters() const { QStringList filters; filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension); return filters; } WimaDataContainer *WimaController::dataContainer() { return _container; } QmlObjectListModel *WimaController::missionItems() { return &_missionItems; } QmlObjectListModel *WimaController::currentMissionItems() { return &_currentMissionItems; } QVariantList WimaController::waypointPath() { return _waypointPath; } QVariantList WimaController::currentWaypointPath() { return _currentWaypointPath; } Fact *WimaController::enableWimaController() { return &_enableWimaController; } Fact *WimaController::overlapWaypoints() { return &_overlapWaypoints; } Fact *WimaController::maxWaypointsPerPhase() { return &_maxWaypointsPerPhase; } Fact *WimaController::showAllMissionItems() { return &_showAllMissionItems; } Fact *WimaController::showCurrentMissionItems() { return &_showCurrentMissionItems; } Fact *WimaController::startWaypointIndex() { return &_startWaypointIndex; } void WimaController::setMasterController(PlanMasterController *masterC) { _masterController = masterC; emit masterControllerChanged(); } void WimaController::setMissionController(MissionController *missionC) { _missionController = missionC; emit missionControllerChanged(); } /*! * \fn void WimaController::setDataContainer(WimaDataContainer *container) * Sets the pointer to the \c WimaDataContainer, which is meant to exchange data between the \c WimaController and the \c WimaPlaner. * * \sa WimaPlaner, WimaDataContainer, WimaPlanData */ void WimaController::setDataContainer(WimaDataContainer *container) { if (container != nullptr) { if (_container != nullptr) { disconnect(_container, &WimaDataContainer::dataValidChanged, this, &WimaController::containerDataValidChanged); } _container = container; connect(_container, &WimaDataContainer::dataValidChanged, this, &WimaController::containerDataValidChanged); emit dataContainerChanged(); } } void WimaController::nextPhase() { updateCurrentMissionItems(); } void WimaController::previousPhase() { if (_startWaypointIndex.rawValue().toInt() > 0) { _startWaypointIndex.setRawValue( _startWaypointIndex.rawValue().toInt() - _maxWaypointsPerPhase.rawValue().toInt() + _overlapWaypoints.rawValue().toInt()); } } void WimaController::resetPhase() { _startWaypointIndex.setRawValue(int(1)); } void WimaController::uploadToVehicle() { _masterController->sendToVehicle(); } void WimaController::removeFromVehicle() { _masterController->removeAllFromVehicle(); } void WimaController::startMission() { } void WimaController::abortMission() { } void WimaController::pauseMission() { } void WimaController::resumeMission() { } void WimaController::saveToCurrent() { } void WimaController::saveToFile(const QString& filename) { QString file = filename; } bool WimaController::loadFromCurrent() { return true; } bool WimaController::loadFromFile(const QString &filename) { QString file = filename; return true; } QJsonDocument WimaController::saveToJson(FileType fileType) { if(fileType) { } return QJsonDocument(); } bool WimaController::calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QList &path) { using namespace GeoUtilities; using namespace PolygonCalculus; QList path2D; bool retVal = PolygonCalculus::shortestPath( toQPolygonF(toCartesian2D(_joinedArea.coordinateList(), /*origin*/ start)), /*start point*/ QPointF(0,0), /*destination*/ toCartesian2D(destination, start), /*shortest path*/ path2D); path.append(toGeo(path2D, /*origin*/ start)); return retVal; } bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QList &coordinateList) { return extractCoordinateList(missionItems, coordinateList, 0, missionItems.count()-1); } bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QList &coordinateList, int startIndex, int endIndex) { if ( startIndex >= 0 && startIndex < missionItems.count() && endIndex >= 0 && endIndex < missionItems.count()) { if (startIndex > endIndex) { if (!extractCoordinateList(missionItems, coordinateList, startIndex, missionItems.count()-1)) return false; if (!extractCoordinateList(missionItems, coordinateList, 0, endIndex)) return false; } else { for (int i = startIndex; i <= endIndex; i++) { SimpleMissionItem *mItem = missionItems.value(i); if (mItem == nullptr) { coordinateList.clear(); return false; } coordinateList.append(mItem->coordinate()); } } } else return false; return true; } bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVariantList &coordinateList) { return extractCoordinateList(missionItems, coordinateList, 0 , missionItems.count()-1); } bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVariantList &coordinateList, int startIndex, int endIndex) { QList geoCoordintateList; bool retValue = extractCoordinateList(missionItems, geoCoordintateList, startIndex, endIndex); if (!retValue) return false; for (auto coordinate : geoCoordintateList) coordinateList.append(QVariant::fromValue(coordinate)); return true; } /*! * \fn void WimaController::containerDataValidChanged(bool valid) * Pulls plan data generated by \c WimaPlaner from the \c _container if the data is valid (\a valid equals true). * Is connected to the dataValidChanged() signal of the \c WimaDataContainer. * * \sa WimaDataContainer, WimaPlaner, WimaPlanData */ void WimaController::containerDataValidChanged(bool valid) { if ( valid ) { if (_container == nullptr) { qWarning("WimaController::containerDataValidChanged(): No container assigned!"); } _localPlanDataValid = false; _visualItems.clear(); _missionItems.clear(); WimaPlanData planData = _container->pull(); // extract list with WimaAreas QList areaList = planData.areaList(); int areaCounter = 0; int numAreas = 4; // extract only numAreas Areas, if there are more they are invalid and ignored for (int i = 0; i < areaList.size(); i++) { const WimaAreaData *areaData = areaList[i]; if (areaData->type() == WimaServiceAreaData::typeString) { // is it a service area? _serviceArea = *qobject_cast(areaData); areaCounter++; _visualItems.append(&_serviceArea); continue; } if (areaData->type() == WimaMeasurementAreaData::typeString) { // is it a measurement area? _measurementArea = *qobject_cast(areaData); areaCounter++; _visualItems.append(&_measurementArea); continue; } if (areaData->type() == WimaCorridorData::typeString) { // is it a corridor? _corridor = *qobject_cast(areaData); areaCounter++; //_visualItems.append(&_corridor); // not needed continue; } if (areaData->type() == WimaJoinedAreaData::typeString) { // is it a corridor? _joinedArea = *qobject_cast(areaData); areaCounter++; _visualItems.append(&_joinedArea); continue; } if (areaCounter >= numAreas) break; } QList tempMissionItems = planData.missionItems(); // create mission items _missionController->removeAll(); QmlObjectListModel* missionControllerVisualItems = _missionController->visualItems(); // find takeoff command int begin = -1; for (int i = 0; i < tempMissionItems.size(); i++) { const MissionItem *missionItem = tempMissionItems[i]; if ( missionItem->command() == MAV_CMD_NAV_VTOL_TAKEOFF || missionItem->command() == MAV_CMD_NAV_TAKEOFF) { _missionController->insertSimpleMissionItem(*missionItem, missionControllerVisualItems->count()); QGeoCoordinate coordinate = missionItem->coordinate(); _takeoffLandPostion.setAltitude(coordinate.altitude()); _takeoffLandPostion.setLatitude(coordinate.latitude()); _takeoffLandPostion.setLongitude(coordinate.longitude()); begin = i + 1; break; } } // check if takeoff command found if (begin < 0) { qWarning("WimaController::containerDataValidChanged(): No takeoff found."); return; } // copy mission items and create SimpleMissionItem by using _missionController for ( int i = begin; i < tempMissionItems.size(); i++) { const MissionItem *missionItem = tempMissionItems[i]; _missionController->insertSimpleMissionItem(*missionItem, missionControllerVisualItems->count()); if ( missionItem->command() == MAV_CMD_NAV_VTOL_LAND || missionItem->command() == MAV_CMD_NAV_LAND) break; } for ( int i = 1; i < missionControllerVisualItems->count(); i++) { SimpleMissionItem *visualItem = qobject_cast((*missionControllerVisualItems)[i]); if (visualItem == nullptr) { qWarning("WimaController::containerDataValidChanged(): Nullptr at SimpleMissionItem!"); return; } SimpleMissionItem *visualItemCopy = new SimpleMissionItem(*visualItem, true, this); _missionItems.append(visualItemCopy); } if (areaCounter == numAreas) _localPlanDataValid = true; updateWaypointPath(); _startWaypointIndex.setRawValue(int(1)); updateCurrentMissionItems(); } else { _localPlanDataValid = false; _visualItems.clear(); _missionItems.clear(); } emit visualItemsChanged(); emit missionItemsChanged(); #ifdef QT_DEBUG //qWarning("Mission Items count: "); //qWarning() << _missionItems.count(); #endif } void WimaController::updateCurrentMissionItems() { int startWaypointIndexInt = _startWaypointIndex.rawValue().toInt()-1; // check if data was fetched and mission end is not reached yet if (_missionItems.count() < 1 || !_localPlanDataValid || startWaypointIndexInt >= _missionItems.count()-2) return; int maxWaypointsPerPhaseInt = _maxWaypointsPerPhase.rawValue().toInt(); // determine end waypoint index _endWaypointIndex = std::min(startWaypointIndexInt + maxWaypointsPerPhaseInt - 1, _missionItems.count()-2); // -2 -> last item is land item // extract waypoints QList geoCoordinateList; // list with potential waypoints (from _missionItems), for _currentMissionItems if (!extractCoordinateList(_missionItems, geoCoordinateList, startWaypointIndexInt, _endWaypointIndex)) { qWarning("WimaController::updateCurrentMissionItems(): error on waypoint extraction."); _currentMissionItems.clear(); return; } // set start waypoint index for next phase if (_endWaypointIndex < _missionItems.count()-2) { _startWaypointIndexList.append(_startWaypointIndex.rawValue().toInt()); disconnect(&_startWaypointIndex, &Fact::rawValueChanged, this, &WimaController::updateCurrentMissionItems); _startWaypointIndex.setRawValue(std::max(_endWaypointIndex + 2 - _overlapWaypoints.rawValue().toInt(), 1)); connect(&_startWaypointIndex, &Fact::rawValueChanged, this, &WimaController::updateCurrentMissionItems); } else { disconnect(&_startWaypointIndex, &Fact::rawValueChanged, this, &WimaController::updateCurrentMissionItems); _startWaypointIndex.setRawValue(_missionItems.count()-1); // marks that end of mission is reached connect(&_startWaypointIndex, &Fact::rawValueChanged, this, &WimaController::updateCurrentMissionItems); } // calculate path from home to first waypoint QList path; if ( !calcShortestPath(_takeoffLandPostion, geoCoordinateList[0], path) ) { qWarning("WimaController::updateCurrentMissionItems(): Not able to calc path from home to first waypoint."); _currentMissionItems.clear(); return; } // prepend to geoCoordinateList for (int i = path.size()-2; i >= 0; i--) // i = path.size()-2 : last coordinate already in geoCoordinateList geoCoordinateList.prepend(path[i]); // calculate path from last waypoint to home path.clear(); if ( !calcShortestPath(geoCoordinateList.last(), _takeoffLandPostion, path) ) { qWarning("WimaController::updateCurrentMissionItems(): Not able to calc path from home to first waypoint."); _currentMissionItems.clear(); return; } path.removeFirst(); // first coordinate already in geoCoordinateList geoCoordinateList.append(path); // create Mission Items _missionController->removeAll(); QmlObjectListModel* missionControllerVisuals = _missionController->visualItems(); // set homeposition of settingsItem MissionSettingsItem* settingsItem = missionControllerVisuals->value(0); if (settingsItem == nullptr) { qWarning("WimaController::updateCurrentMissionItems(): nullptr"); _currentMissionItems.clear(); return; } settingsItem->setCoordinate(_takeoffLandPostion); for (auto coordinate : geoCoordinateList) { _missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count()); } // set homeposition for take off item (somehow not working with insertSimpleMissionItem) SimpleMissionItem* takeoff = missionControllerVisuals->value(1); if (takeoff == nullptr) { qWarning("WimaController::updateCurrentMissionItems(): nullptr"); _currentMissionItems.clear(); return; } takeoff->setCoordinate(_takeoffLandPostion); // set land command for last mission item SimpleMissionItem *landItem = missionControllerVisuals->value(missionControllerVisuals->count()-1); if (landItem == nullptr) { qWarning("WimaController::updateCurrentMissionItems(): nullptr"); _currentMissionItems.clear(); return; } // check vehicle type, before setting land command Vehicle* controllerVehicle = _masterController->controllerVehicle(); MAV_CMD landCmd = controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_LAND; if (controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) { landItem->setCommand(landCmd); } else { _currentMissionItems.clear(); return; } // copy to _currentMissionItems _currentMissionItems.clear(); for ( int i = 1; i < missionControllerVisuals->count(); i++) { SimpleMissionItem *visualItem = missionControllerVisuals->value(i); if (visualItem == nullptr) { qWarning("WimaController::updateCurrentMissionItems(): Nullptr at SimpleMissionItem!"); _currentMissionItems.clear(); return; } SimpleMissionItem *visualItemCopy = new SimpleMissionItem(*visualItem, true, this); _currentMissionItems.append(visualItemCopy); } updateCurrentPath(); emit currentMissionItemsChanged(); } void WimaController::updateWaypointPath() { _waypointPath.clear(); extractCoordinateList(_missionItems, _waypointPath, 0, _missionItems.count()-1); emit waypointPathChanged(); } void WimaController::updateCurrentPath() { _currentWaypointPath.clear(); extractCoordinateList(_currentMissionItems, _currentWaypointPath, 0, _currentMissionItems.count()-1); emit currentWaypointPathChanged(); } void WimaController::updateNextWaypoint() { if (_endWaypointIndex < _missionItems.count()-2) { disconnect(&_startWaypointIndex, &Fact::rawValueChanged, this, &WimaController::updateCurrentMissionItems); _startWaypointIndex.setRawValue(_endWaypointIndex + 2 - _overlapWaypoints.rawValue().toInt()); connect(&_startWaypointIndex, &Fact::rawValueChanged, this, &WimaController::updateCurrentMissionItems); } }