#include "DefaultManager.h" #include "Wima/Geometry/GeoUtilities.h" #include "Wima/Geometry/PolygonCalculus.h" #include "MissionSettingsItem.h" WaypointManager::DefaultManager::DefaultManager(Settings &settings, AreaInterface &interface) : ManagerBase(settings) , _areaInterface(&interface) { } void WaypointManager::DefaultManager::clear() { _dirty = true; _waypoints.clear(); _currentWaypoints.clear(); _missionItems.clearAndDeleteContents(); _currentMissionItems.clearAndDeleteContents(); _waypointsVariant.clear(); _currentWaypointsVariant.clear(); } bool WaypointManager::DefaultManager::update() { // extract waypoints _currentWaypoints.clear(); Slicer::update(_waypoints, _currentWaypoints); return _worker(); } bool WaypointManager::DefaultManager::next() { // extract waypoints _currentWaypoints.clear(); Slicer::next(_waypoints, _currentWaypoints); return _worker(); } bool WaypointManager::DefaultManager::previous() { // extract waypoints _currentWaypoints.clear(); Slicer::previous(_waypoints, _currentWaypoints); return _worker(); } bool WaypointManager::DefaultManager::reset() { // extract waypoints _currentWaypoints.clear(); Slicer::reset(_waypoints, _currentWaypoints); return _worker(); } bool WaypointManager::DefaultManager::_insertMissionItem(const QGeoCoordinate &c, size_t index, QmlObjectListModel &list, bool doUpdate) { using namespace WaypointManager::Utils; if ( !insertMissionItem(c, index /*insertion index*/, list, _settings->vehicle(), _settings->isFlyView(), &_currentMissionItems /*parent*/, doUpdate /*do update*/) ) { Q_ASSERT(false); qWarning("WaypointManager::DefaultManager::next(): insertMissionItem failed."); return false; } return true; } bool WaypointManager::DefaultManager::_insertMissionItem(const QGeoCoordinate &c, size_t index, bool doUpdate) { return _insertMissionItem(c, index, _currentMissionItems, doUpdate); } bool WaypointManager::DefaultManager::_calcShortestPath( const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector &path) { using namespace GeoUtilities; using namespace PolygonCalculus; QVector path2D; bool retVal = PolygonCalculus::shortestPath( toQPolygonF(toCartesian2D(_areaInterface->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 WaypointManager::DefaultManager::_worker() { using namespace WaypointManager::Utils; if (_waypoints.count() < 1 || !_settings->valid()) { return false; } if (_dirty) { _missionItems.clearAndDeleteContents(); _waypointsVariant.clear(); initialize(_missionItems, _settings->vehicle(), _settings->isFlyView()); for (size_t i = 0; i < size_t(_waypoints.size()); ++i) { const QGeoCoordinate &c = _waypoints.at(i); _insertMissionItem(c, _missionItems.count(), _missionItems, false /*update list*/); _waypointsVariant.push_back(QVariant::fromValue(c)); } Q_ASSERT(_missionItems.value(0) != nullptr); doUpdate(_missionItems); _dirty = false; } _currentMissionItems.clearAndDeleteContents(); initialize(_currentMissionItems, _settings->vehicle(), _settings->isFlyView()); // calculate path from home to first waypoint QVector arrivalPath; if (!_settings->homePosition().isValid()){ Q_ASSERT(false); qWarning("WaypointManager::DefaultManager::next(): home position invalid."); return false; } if ( !_calcShortestPath(_settings->homePosition(), _currentWaypoints.first(), arrivalPath) ) { Q_ASSERT(false); qWarning("WaypointManager::DefaultManager::next(): Not able to calc path from home position to first waypoint."); return false; } arrivalPath.removeFirst(); // calculate path from last waypoint to home qDebug() << "_currentWaypoints.size(): " << _currentWaypoints.size(); qDebug() << "_currentWaypoints.first(): " << _currentWaypoints.first(); qDebug() << "_currentWaypoints.last(): " << _currentWaypoints.last(); qDebug() << "_settings->homePosition(): " << _settings->homePosition(); QVector returnPath; //if ( !_calcShortestPath(_currentWaypoints.last(), _settings->homePosition(), returnPath) ) { if ( !_calcShortestPath(_settings->homePosition(), _currentWaypoints.first(), returnPath) ) { Q_ASSERT(false); qWarning("WaypointManager::DefaultManager::next(): Not able to calc path from home position to last waypoint."); //return false; } returnPath.removeFirst(); returnPath.removeLast(); // Create mission items. // Set home position of MissionSettingsItem. MissionSettingsItem* settingsItem = _currentMissionItems.value(0); if (settingsItem == nullptr) { Q_ASSERT(false); qWarning("WaypointManager::DefaultManager::next(): nullptr."); return false; } settingsItem->setCoordinate(_settings->homePosition()); // Set takeoff position for first mission item (bug). _insertMissionItem(_settings->homePosition(), 1 /*insertion index*/, false /*do update*/); SimpleMissionItem *takeoffItem = _currentMissionItems.value(1); if (takeoffItem == nullptr) { Q_ASSERT(false); qWarning("WaypointManager::DefaultManager::next(): nullptr."); return false; } takeoffItem->setCoordinate(_settings->homePosition()); // Create change speed item. _insertMissionItem(_settings->homePosition(), 2 /*insertion index*/, false /*do update*/); SimpleMissionItem *speedItem = _currentMissionItems.value(2); if (speedItem == nullptr) { Q_ASSERT(false); qWarning("WaypointManager::DefaultManager::next(): nullptr."); return false; } speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // Set coordinate must be after setCommand (setCommand sets coordinate to zero). speedItem->setCoordinate(_settings->homePosition()); speedItem->missionItem().setParam2(_settings->arrivalReturnSpeed()); // insert arrival path for (auto coordinate : arrivalPath) { _insertMissionItem(coordinate, _currentMissionItems.count() /*insertion index*/, false /*do update*/); } // Create change speed item (after arrival path). int index = _currentMissionItems.count(); _insertMissionItem(_currentWaypoints.first(), index /*insertion index*/, false /*do update*/); speedItem = _currentMissionItems.value(index); if (speedItem == nullptr) { Q_ASSERT(false); qWarning("WaypointManager::DefaultManager::next(): nullptr."); return false; } speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero) speedItem->setCoordinate(_currentWaypoints.first()); speedItem->missionItem().setParam2(_settings->flightSpeed()); // Insert slice. for (auto coordinate : _currentWaypoints) { _insertMissionItem(coordinate, _currentMissionItems.count() /*insertion index*/, false /*do update*/); } // Create change speed item, after slice. index = _currentMissionItems.count(); _insertMissionItem(_currentWaypoints.last(), index /*insertion index*/, false /*do update*/); speedItem = _currentMissionItems.value(index); if (speedItem == nullptr) { Q_ASSERT(false); qWarning("WaypointManager::DefaultManager::next(): nullptr."); return false; } speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero) speedItem->setCoordinate(_currentWaypoints.last()); speedItem->missionItem().setParam2(_settings->arrivalReturnSpeed()); // Insert return path coordinates. for (auto coordinate : returnPath) { _insertMissionItem(coordinate, _currentMissionItems.count() /*insertion index*/, false /*do update*/); } // Set land command for last mission item. index = _currentMissionItems.count(); _insertMissionItem(_settings->homePosition(), index /*insertion index*/, false /*do update*/); SimpleMissionItem *landItem = _currentMissionItems.value(index); if (landItem == nullptr) { Q_ASSERT(false); qWarning("WaypointManager::DefaultManager::next(): nullptr."); return false; } MAV_CMD landCmd = _settings->vehicle()->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_LAND; if (_settings->vehicle()->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) { landItem->setCommand(landCmd); } else { Q_ASSERT(false); qWarning("WaypointManager::DefaultManager::next(): Land command not supported!"); return false; } // Prepend arrival path to slice. for ( long i = arrivalPath.size()-1; i >=0; --i ) _currentWaypoints.push_front(arrivalPath[i]); // Append return path to slice. for ( auto c : returnPath ) _currentWaypoints.push_back(c); _currentWaypointsVariant.clear(); for ( auto c : _currentWaypoints) _currentWaypointsVariant.push_back(QVariant::fromValue(c)); // Set altitude. for (int i = 0; i < _currentMissionItems.count(); ++i) { SimpleMissionItem *item = _currentMissionItems.value(i); if (item == nullptr) { Q_ASSERT(false); qWarning("WimaController::updateAltitude(): nullptr"); return false; } item->altitude()->setRawValue(_settings->altitude()); } return true; }