diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 4461381442ac8c243ca047db108b466a4afaeb33..60bdb0bf83744f784ef8d0abb7d01636c77d6924 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -1053,9 +1053,9 @@ bool MissionController::readyForSaveSend(void) const return true; } -bool MissionController::setTakeoffCommand(SimpleMissionItem &missionItem, Vehicle &vehicle) +bool MissionController::setTakeoffCommand(SimpleMissionItem &missionItem) { - if (vehicle.fixedWing() || vehicle.vtol() || vehicle.multiRotor()) { + if (_controllerVehicle->fixedWing() || _controllerVehicle->vtol() || _controllerVehicle->multiRotor()) { MAV_CMD takeoffCmd = _controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_TAKEOFF : MAV_CMD_NAV_TAKEOFF; if (_controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(takeoffCmd)) { missionItem.setCommand(takeoffCmd); @@ -1067,10 +1067,10 @@ bool MissionController::setTakeoffCommand(SimpleMissionItem &missionItem, Vehicl return true; } -bool MissionController::setLandCommand(SimpleMissionItem &missionItem, Vehicle &vehicle) +bool MissionController::setLandCommand(SimpleMissionItem &missionItem) { - MAV_CMD landCmd = vehicle.vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_LAND; - if (vehicle.firmwarePlugin()->supportedMissionCommands().contains(landCmd)) { + MAV_CMD landCmd = _controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_LAND; + if (_controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) { missionItem.setCommand(landCmd); } else return false; diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 0ed16524d2ae17965082e8043bd0854bdfebfd71..a0c7246d524b6366a673592afda288f42cbf01da 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -151,9 +151,9 @@ public: bool readyForSaveSend(void) const; /// sets the command in missionItem to a land command - bool setLandCommand (SimpleMissionItem &missionItem, Vehicle &vehicle); + bool setLandCommand (SimpleMissionItem &missionItem); /// sets the command in missionItem to a takeoff command - bool setTakeoffCommand (SimpleMissionItem &missionItem, Vehicle &vehicle); + bool setTakeoffCommand (SimpleMissionItem &missionItem); /// Sends the mission items to the specified vehicle static void sendItemsToVehicle(Vehicle* vehicle, QmlObjectListModel* visualMissionItems); diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 3e48f46c9e3cfeeb9d73637e6551fa2e08678a71..347ea82a9c86c4a72ba43a8fd82a066841745595 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -548,7 +548,7 @@ bool WimaController::calcNextPhase() qWarning("WimaController::calcNextPhase(): nullptr"); return false; } - _missionController->setLandCommand(*landItem, *_masterController->controllerVehicle()); + _missionController->setLandCommand(*landItem); // copy to _currentMissionItems for ( int i = 1; i < missionControllerVisuals->count(); i++) { diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index 5e2106d34054c1e0b592a95c5533c1fd4ef1bd6e..b2df41886b66c2e7343391d66457405fad9c09d4 100644 --- a/src/Wima/WimaPlaner.cc +++ b/src/Wima/WimaPlaner.cc @@ -17,8 +17,13 @@ WimaPlaner::WimaPlaner(QObject *parent) , _measurementArea (this) , _serviceArea (this) , _corridor (this) + , _circularSurvey (nullptr) + , _surveyRefChanging (false) { connect(this, &WimaPlaner::currentPolygonIndexChanged, this, &WimaPlaner::recalcPolygonInteractivity); + connect(&_updateTimer, &QTimer::timeout, this, &WimaPlaner::updateTimerSlot); + _updateTimer.setInterval(250); // 250 ms means: max update time 2*250 ms + _updateTimer.start(); } QmlObjectListModel* WimaPlaner::visualItems() @@ -195,120 +200,33 @@ bool WimaPlaner::updateMission() // extract old survey data QmlObjectListModel* missionItems = _missionController->visualItems(); - CircularSurveyComplexItem* survey = nullptr; - int surveyIndex = -1; - - for (int i = 0; i < _missionController->visualItems()->count(); i++) { - survey = qobject_cast(missionItems->get(i)); - if ( survey != nullptr) { - surveyIndex = i; - break; - } - - } + int surveyIndex = missionItems->indexOf(_circularSurvey); // create survey item if not yet present - if (surveyIndex < 0) { - // set home position to serArea center - MissionSettingsItem* settingsItem= qobject_cast(missionItems->get(0)); - if (settingsItem == nullptr){ - qWarning("WimaPlaner::updateMission(): settingsItem == nullptr"); - return false; - } - // set altitudes, temporary measure to solve bugs - QGeoCoordinate center = _serviceArea.center(); - center.setAltitude(0); - _serviceArea.setCenter(center); - center = _measurementArea.center(); - center.setAltitude(0); - _measurementArea.setCenter(center); - center = _corridor.center(); - center.setAltitude(0); - _corridor.setCenter(center); - // set HomePos. to serArea center - settingsItem->setCoordinate(_serviceArea.center()); - - // create take off position item - int sequenceNumber = _missionController->insertSimpleMissionItem(_serviceArea.center(), missionItems->count()); - - _missionController->setCurrentPlanViewIndex(sequenceNumber, true); - SimpleMissionItem* takeOffItem = qobject_cast(missionItems->get(missionItems->count()-1)); - if (takeOffItem == nullptr){ - qWarning("WimaPlaner::updateMission(): takeOffItem == nullptr"); - return false; - } - takeOffItem->setCoordinate(_serviceArea.center()); //necessary, insertSimpleMissionItem does not copy coordinate (why?) - - + if (surveyIndex == -1) { _missionController->insertComplexMissionItem(_missionController->circularSurveyComplexItemName(), _measurementArea.center(), missionItems->count()); - survey = qobject_cast(missionItems->get(missionItems->count()-1)); + _circularSurvey = qobject_cast(missionItems->get(missionItems->count()-1)); - if (survey == nullptr){ + if (_circularSurvey == nullptr){ qWarning("WimaPlaner::updateMission(): survey == nullptr"); return false; } - survey->setRefPoint(_measurementArea.center()); - survey->setAutoGenerated(true); // prevents reinitialisation from gui - connect(survey->deltaR(), &Fact::rawValueChanged, this, &WimaPlaner::setDirtyTrue); - connect(survey->deltaAlpha(), &Fact::rawValueChanged, this, &WimaPlaner::setDirtyTrue); - connect(survey->isSnakePath(), &Fact::rawValueChanged, this, &WimaPlaner::setDirtyTrue); - connect(survey->transectMinLength(), &Fact::rawValueChanged, this, &WimaPlaner::setDirtyTrue); + // establish connections + _circularSurvey->setRefPoint(_measurementArea.center()); + _circularSurvey->setAutoGenerated(true); // prevents reinitialisation from gui + connect(_circularSurvey->deltaR(), &Fact::rawValueChanged, this, &WimaPlaner::calcArrivalAndReturnPath); + connect(_circularSurvey->deltaAlpha(), &Fact::rawValueChanged, this, &WimaPlaner::calcArrivalAndReturnPath); + connect(_circularSurvey->isSnakePath(), &Fact::rawValueChanged, this, &WimaPlaner::calcArrivalAndReturnPath); + connect(_circularSurvey->transectMinLength(), &Fact::rawValueChanged, this, &WimaPlaner::calcArrivalAndReturnPath); } - survey->surveyAreaPolygon()->clear(); - survey->surveyAreaPolygon()->appendVertices(_measurementArea.coordinateList()); + // update survey area + _circularSurvey->surveyAreaPolygon()->clear(); + _circularSurvey->surveyAreaPolygon()->appendVertices(_measurementArea.coordinateList()); - // calculate path from take off to opArea - if (survey->visualTransectPoints().size() == 0) { - qWarning("WimaPlaner::updateMission(): survey no points."); - return false; - } - QGeoCoordinate start = _serviceArea.center(); - QGeoCoordinate end = survey->coordinate(); - -#ifdef QT_DEBUG - if (!_visualItems.contains(&_joinedArea)) - _visualItems.append(&_joinedArea); -#endif - - QList path; - if ( !calcShortestPath(start, end, path)) { - qgcApp()->showMessage( QString(tr("Not able to calculate the path from takeoff position to measurement area.")).toLocal8Bit().data()); - return false; - } - _arrivalPathLength = path.size()-1; // -1: last item is first measurement point - for (int i = 1; i < path.count()-1; i++) { - sequenceNumber = _missionController->insertSimpleMissionItem(path.value(i), missionItems->count()-1); - _missionController->setCurrentPlanViewIndex(sequenceNumber, true); - } - - // calculate return path - start = survey->exitCoordinate(); - end = _serviceArea.center(); - path.clear(); - if ( !calcShortestPath(start, end, path)) { - qgcApp()->showMessage(QString(tr("Not able to calculate the path from measurement area to landing position.")).toLocal8Bit().data()); - return false; - } - _returnPathLength = path.size()-1; // -1: fist item is last measurement point - for (int i = 1; i < path.count()-1; i++) { - sequenceNumber = _missionController->insertSimpleMissionItem(path.value(i), missionItems->count()); - _missionController->setCurrentPlanViewIndex(sequenceNumber, true); - } - - // create land position item - sequenceNumber = _missionController->insertSimpleMissionItem(_serviceArea.center(), missionItems->count()); - _missionController->setCurrentPlanViewIndex(sequenceNumber, true); - SimpleMissionItem* landItem = qobject_cast(missionItems->get(missionItems->count()-1)); - if (landItem == nullptr){ - qWarning("WimaPlaner::updateMission(): landItem == nullptr"); - return false; - } else { - if (!_missionController->setLandCommand(*landItem, *_masterController->controllerVehicle())) - return false; - } + calcArrivalAndReturnPath(); pushToContainer(); // exchange plan data with the WimaController via the _container setDirty(false); @@ -524,6 +442,116 @@ void WimaPlaner::recalcPolygonInteractivity(int index) } } +bool WimaPlaner::calcArrivalAndReturnPath() +{ + // extract old survey data + QmlObjectListModel *missionItems = _missionController->visualItems(); + + int surveyIndex = missionItems->indexOf(_circularSurvey); + + if (surveyIndex == -1) { + qWarning("WimaPlaner::calcArrivalAndReturnPath(): no survey item"); + return false; + } + + // remove old arrival and return path + int size = missionItems->count(); + for (int i = surveyIndex+1; i < size; i++) + _missionController->removeMissionItem(surveyIndex+1); + for (int i = surveyIndex-1; i > 1; i--) + _missionController->removeMissionItem(i); + + // set home position to serArea center + MissionSettingsItem* settingsItem= qobject_cast(missionItems->get(0)); + if (settingsItem == nullptr){ + qWarning("WimaPlaner::calcArrivalAndReturnPath(): settingsItem == nullptr"); + return false; + } + + // set altitudes, temporary measure to solve bugs + QGeoCoordinate center = _serviceArea.center(); + center.setAltitude(0); + _serviceArea.setCenter(center); + center = _measurementArea.center(); + center.setAltitude(0); + _measurementArea.setCenter(center); + center = _corridor.center(); + center.setAltitude(0); + _corridor.setCenter(center); + // set HomePos. to serArea center + settingsItem->setCoordinate(_serviceArea.center()); + + // set takeoff position + bool setCommandNeeded = false; + if (missionItems->count() < 3) { + setCommandNeeded = true; + _missionController->insertSimpleMissionItem(_serviceArea.center(), 1); + } + SimpleMissionItem* takeOffItem = qobject_cast(missionItems->get(1)); + if (takeOffItem == nullptr){ + qWarning("WimaPlaner::calcArrivalAndReturnPath(): takeOffItem == nullptr"); + return false; + } + if (setCommandNeeded) + _missionController->setTakeoffCommand(*takeOffItem); + takeOffItem->setCoordinate(_serviceArea.center()); + + if (_circularSurvey->visualTransectPoints().size() == 0) { + qWarning("WimaPlaner::calcArrivalAndReturnPath(): survey no points."); + return false; + } + + // calculate path from take off to survey + QGeoCoordinate start = _serviceArea.center(); + QGeoCoordinate end = _circularSurvey->coordinate(); + + #ifdef QT_DEBUG + if (!_visualItems.contains(&_joinedArea)) + _visualItems.append(&_joinedArea); + #endif + + QList path; + if ( !calcShortestPath(start, end, path)) { + qgcApp()->showMessage( QString(tr("Not able to calculate the path from takeoff position to measurement area.")).toLocal8Bit().data()); + return false; + } + _arrivalPathLength = path.size()-1; // -1: last item is first measurement point + int sequenceNumber = 0; + for (int i = 1; i < path.count()-1; i++) { + sequenceNumber = _missionController->insertSimpleMissionItem(path.value(i), missionItems->count()-1); + _missionController->setCurrentPlanViewIndex(sequenceNumber, true); + } + + // calculate return path + start = _circularSurvey->exitCoordinate(); + end = _serviceArea.center(); + path.clear(); + if ( !calcShortestPath(start, end, path)) { + qgcApp()->showMessage(QString(tr("Not able to calculate the path from measurement area to landing position.")).toLocal8Bit().data()); + return false; + } + _returnPathLength = path.size()-1; // -1: fist item is last measurement point + for (int i = 1; i < path.count()-1; i++) { + sequenceNumber = _missionController->insertSimpleMissionItem(path.value(i), missionItems->count()); + _missionController->setCurrentPlanViewIndex(sequenceNumber, true); + } + + // create land position item + sequenceNumber = _missionController->insertSimpleMissionItem(_serviceArea.center(), missionItems->count()); + _missionController->setCurrentPlanViewIndex(sequenceNumber, true); + SimpleMissionItem* landItem = qobject_cast(missionItems->get(missionItems->count()-1)); + if (landItem == nullptr){ + qWarning("WimaPlaner::calcArrivalAndReturnPath(): landItem == nullptr"); + return false; + } else { + if (!_missionController->setLandCommand(*landItem)) + return false; + } + + + return true; +} + bool WimaPlaner::recalcJoinedArea(QString &errorString) { // check if area paths form simple polygons @@ -652,6 +680,30 @@ void WimaPlaner::setDirtyTrue() setDirty(true); } +void WimaPlaner::updateTimerSlot() +{ + // General operation of this function: + // Check if parameter has changed, wait until it stops changing, update mission + + // circular survey reference point + if (_circularSurvey != nullptr) { + if (_surveyRefChanging) { + if (_circularSurvey->refPoint() == _lastSurveyRefPoint) { // is it still changing? + calcArrivalAndReturnPath(); + _surveyRefChanging = false; + } + } else { + if (_circularSurvey->refPoint() != _lastSurveyRefPoint) // does it started changing? + _surveyRefChanging = true; + } + } + + + // update old values + if (_circularSurvey != nullptr) + _lastSurveyRefPoint = _circularSurvey->refPoint() ; +} + QJsonDocument WimaPlaner::saveToJson(FileType fileType) { /// This function save all areas (of WimaPlaner) and all mission items (of MissionController) to a QJsonDocument diff --git a/src/Wima/WimaPlaner.h b/src/Wima/WimaPlaner.h index c03855d0ce0543ec2674c4bd24bf76ca0b053d9c..85f12b22372a21a6e98f98ffd13434a3a70d78d9 100644 --- a/src/Wima/WimaPlaner.h +++ b/src/Wima/WimaPlaner.h @@ -122,10 +122,12 @@ signals: private slots: void recalcPolygonInteractivity (int index); + bool calcArrivalAndReturnPath (void); bool recalcJoinedArea (QString &errorString); void pushToContainer (); void setDirtyTrue (); - + // called by _updateTimer::timeout signal, updates different mission parts, if parameters (e.g. survey or areas) have changed + void updateTimerSlot (); private: // Member Functions WimaPlanData toPlanData(); @@ -145,4 +147,10 @@ private: WimaCorridor _corridor; // corridor connecting _measurementArea and _serviceArea int _arrivalPathLength; // the number waypoints the arrival path consists of (path from takeoff to first measurement point) int _returnPathLength; // the number waypoints the return path consists of (path from last measurement point to land) + + CircularSurveyComplexItem* _circularSurvey; // pointer to the CircularSurvey item in _missionController.visualItems() + + QTimer _updateTimer; // on this timers timeout different mission parts will be updated, if parameters (e.g. survey or areas) have changed + QGeoCoordinate _lastSurveyRefPoint; + bool _surveyRefChanging; };