diff --git a/src/FlightDisplay/FlightDisplayWimaMenu.qml b/src/FlightDisplay/FlightDisplayWimaMenu.qml index 352dd7d0d413f7c83e5aacab9457ecd1e2c57eb1..a13dfc4322b063926ea201228a2ac5106fda7dbb 100644 --- a/src/FlightDisplay/FlightDisplayWimaMenu.qml +++ b/src/FlightDisplay/FlightDisplayWimaMenu.qml @@ -19,14 +19,16 @@ import QGroundControl.FactControls 1.0 Item { id: _root - height: 600 - width: 300 - + height: mainFrame.height + width: mainFrame.width + property var maxHeight: 500 + property var maxWidth: 300 property var wimaController // must be provided by the user property var planMasterController // must be provided by the user property bool _controllerValid: planMasterController !== undefined property real _controllerProgressPct: _controllerValid ? planMasterController.missionController.progressPct : 0 + property double _margins: ScreenTools.defaultPixelWidth*0.3 signal initSmartRTL(); @@ -58,22 +60,48 @@ Item { id: mainFrame anchors.left: parent.left anchors.bottom: parent.bottom - height: enableWima.enableWimaBoolean ? parent.height : enableWima.height - width: enableWima.enableWimaBoolean ? parent.width : enableWima.width + height: enableWima.enableWimaBoolean ? Math.min(parent.maxHeight, enableWima.height + flickable.contentHeight + 2*_margins) : enableWima.height + _margins + width: enableWima.enableWimaBoolean ? Math.min(parent.maxWidth, flickable.contentWidth + 2*_margins) : enableWima.width color: enableWima.enableWimaBoolean ? qgcPal.window : "transparent" radius: ScreenTools.defaultFontPixelHeight / 4 + clip: false + + Component.onCompleted: { + console.log('onCompleted') + console.log('height') + console.log(height) + console.log('width') + console.log(width) + } + + onHeightChanged: { + console.log('height') + console.log(height) + _root.height = mainFrame.height + } + + onWidthChanged: { + console.log('width') + console.log(width) + _root.width = mainFrame.width + } // checkbox to enable/ disable wima SliderSwitch { id: enableWima anchors.horizontalCenter: parent.horizontalCenter - anchors.topMargin: ScreenTools.defaultFontPixelHeight * 0.25 + anchors.topMargin: _margins anchors.top: parent.top confirmText: enableWimaBoolean ? qsTr("disable WiMA") : qsTr("enable WiMA") + property var enableWimaFact: wimaController.enableWimaController property bool enableWimaBoolean: enableWimaFact.value + Component.onCompleted: { + enableWimaFact.value = false + } + onAccept: { if (enableWimaBoolean) { enableWimaFact.value = false @@ -83,252 +111,276 @@ Item { } } - Item { - id: flickableWrapper + /*MouseArea { + anchors.fill: enableWima + + hoverEnabled: true + + property var enableWimaFact: wimaController.enableWimaController + property bool enableWimaBoolean: enableWimaFact.value + onEntered: { + timer.stop() + enableWima.visible = true + } + + onExited: { + if (enableWimaBoolean === false) { + timer.start() + } + } + } + + Timer { + id: timer + interval: 1000 + running: false + repeat: false + onTriggered: enableWima.visible = false + }*/ + + + QGCFlickable { + id: flickable + clip: false anchors.top: enableWima.bottom anchors.left: parent.left - anchors.right: parent.right - anchors.bottom: parent.bottom - anchors.margins: ScreenTools.defaultFontPixelHeight*0.5 - - QGCFlickable { - clip: true - anchors.fill: parent - contentHeight: wrapperItem.height - contentWidth: wrapperItem.width - - Item { - id: wrapperItem - width: Math.max(flickableWrapper.width, mainColumn.width) - height: mainColumn.height - - property int itemWidth: 120 - - Column { - id: mainColumn - anchors.horizontalCenter: parent.horizontalCenter - spacing: ScreenTools.defaultFontPixelHeight * 0.3 - - SectionHeader{ - id: settingsHeader - text: qsTr("Settings") - } - GridLayout { - columns: 2 - rowSpacing: ScreenTools.defaultFontPixelHeight * 0.5 - columnSpacing: ScreenTools.defaultFontPixelHeight * 0.5 - visible: settingsHeader.checked - - QGCLabel { - text: qsTr("Next Waypoint") - Layout.fillWidth: true - } - FactTextField { - fact: wimaController.startWaypointIndex - Layout.fillWidth: true - } - - QGCLabel { - text: qsTr("Max Waypoints") - Layout.fillWidth: true - } - FactTextField { - fact: wimaController.maxWaypointsPerPhase - Layout.fillWidth: true - } - - QGCLabel { - text: qsTr("Overlap") - Layout.fillWidth: true - } - FactTextField { - fact: wimaController.overlapWaypoints - Layout.fillWidth: true - } - } - GridLayout { - columns: 2 - rowSpacing: ScreenTools.defaultFontPixelHeight * 0.5 - columnSpacing: ScreenTools.defaultFontPixelHeight * 0.5 - visible: settingsHeader.checked - - FactCheckBox { - text: qsTr("Show All") - fact: wimaController.showAllMissionItems - Layout.fillWidth: true - } - - FactCheckBox { - text: qsTr("Show Current") - fact: wimaController.showCurrentMissionItems - Layout.fillWidth: true - } - - FactCheckBox { - text: qsTr("Reverse") - fact: wimaController.reverse - Layout.fillWidth: true - } - } // Grid - - SectionHeader{ - id: missionHeader - text: qsTr("Mission") - } - GridLayout { - columns: 2 - rowSpacing: ScreenTools.defaultFontPixelHeight * 0.5 - columnSpacing: ScreenTools.defaultFontPixelHeight * 0.5 - visible: missionHeader.checked - width: parent.width - - QGCLabel { - text: qsTr("Speed") - Layout.fillWidth: true - } - FactTextField { - fact: wimaController.flightSpeed - Layout.fillWidth: true - } - - QGCLabel { - text: qsTr("Altitude") - Layout.fillWidth: true - } - FactTextField { - fact: wimaController.altitude - Layout.fillWidth: true - } - - } - - GridLayout { - columns: 2 - rowSpacing: ScreenTools.defaultFontPixelHeight * 0.5 - columnSpacing: ScreenTools.defaultFontPixelHeight * 0.5 - visible: missionHeader.checked - width: parent.width - - // Buttons - QGCButton { - id: buttonPreviousMissionPhase - text: qsTr("Reverse") - onClicked: wimaController.previousPhase() - Layout.fillWidth: true - } - - QGCButton { - id: buttonNextMissionPhase - text: qsTr("Forward") - onClicked: wimaController.nextPhase() - Layout.fillWidth: true - } - - QGCButton { - id: buttonResetPhase - text: qsTr("Reset Phase") - onClicked: wimaController.resetPhase(); - Layout.columnSpan: 2 - Layout.fillWidth: true - } - } // Grid - - SectionHeader{ - id: vehicleHeader - text: qsTr("Vehicle") - } - GridLayout { - columns: 2 - rowSpacing: ScreenTools.defaultFontPixelHeight * 0.5 - columnSpacing: ScreenTools.defaultFontPixelHeight * 0.5 - visible: vehicleHeader.checked - width: parent.width - - QGCButton { - id: buttonUpload - text: qsTr("Upload") - onClicked: wimaController.uploadToVehicle() - Layout.fillWidth: true - } - - QGCButton { - id: buttonRemoveFromVehicle - text: qsTr("Remove") - onClicked: wimaController.removeFromVehicle() - Layout.fillWidth: true - } - - QGCButton { - id: buttonSmartRTL - text: qsTr("Smart RTL") - onClicked: initSmartRTL() - Layout.columnSpan: 2 - Layout.fillWidth: true - } - - - // progess bar - Rectangle { - id: progressBar - height: 4 - width: _controllerProgressPct * parent.width - color: qgcPal.colorGreen - visible: false - Layout.columnSpan: 2 - Layout.fillWidth: true - } - - QGCLabel { - id: uploadCompleteText - font.pointSize: ScreenTools.largeFontPointSize - Layout.columnSpan: 2 - horizontalAlignment: Text.AlignHCenter - verticalAlignment: Text.AlignVCenter - text: "Done" - visible: false - Layout.fillWidth: true - } - } - - SectionHeader { - id: statsHeader - text: qsTr("Statistics") - } - GridLayout { - columns: 2 - rowSpacing: ScreenTools.defaultFontPixelHeight * 0.5 - anchors.topMargin: ScreenTools.defaultFontPixelHeight * 0.5 - visible: statsHeader.checked - - QGCLabel { - text: qsTr("Phase Length: ") - wrapMode: Text.WordWrap - Layout.fillWidth: true - } - QGCLabel { - text: wimaController.phaseDistance >= 0 ? wimaController.phaseDistance.toFixed(2) + " m": "" - wrapMode: Text.WordWrap - Layout.fillWidth: true - } - - QGCLabel { - text: qsTr("Phase Duration: ") - wrapMode: Text.WordWrap - Layout.fillWidth: true - } - QGCLabel { - text: wimaController.phaseDuration >= 0 ? getTime(wimaController.phaseDuration) : "" - wrapMode: Text.WordWrap - Layout.fillWidth: true - } - QGCLabel { - text: "" - Layout.columnSpan: 2 - } - } - } // settingsColumn - } // wrapperItem - } // QGCFlickable - } // Item + contentHeight: mainColumn.height + contentWidth: mainColumn.width + + onContentHeightChanged: { + console.log('contentHeight') + console.log(contentHeight) + mainFrame.height = enableWima.enableWimaBoolean ? Math.min(parent.maxHeight, enableWima.height + flickable.contentHeight + 2*_margins) : enableWima.height + _margins + } + + onContentWidthChanged: { + console.log('contentWidth') + console.log(contentWidth) + mainFrame.width = enableWima.enableWimaBoolean ? Math.min(parent.maxWidth, flickable.contentWidth + 2*_margins) : enableWima.width + } + Column { + id: mainColumn + anchors.horizontalCenter: parent.horizontalCenter + spacing: ScreenTools.defaultFontPixelHeight * 0.3 + + SectionHeader{ + id: settingsHeader + text: qsTr("Settings") + } + GridLayout { + columns: 2 + rowSpacing: ScreenTools.defaultFontPixelHeight * 0.5 + columnSpacing: ScreenTools.defaultFontPixelHeight * 0.5 + visible: settingsHeader.checked + + QGCLabel { + text: qsTr("Next Waypoint") + Layout.fillWidth: true + } + FactTextField { + fact: wimaController.startWaypointIndex + Layout.fillWidth: true + } + + QGCLabel { + text: qsTr("Max Waypoints") + Layout.fillWidth: true + } + FactTextField { + fact: wimaController.maxWaypointsPerPhase + Layout.fillWidth: true + } + + QGCLabel { + text: qsTr("Overlap") + Layout.fillWidth: true + } + FactTextField { + fact: wimaController.overlapWaypoints + Layout.fillWidth: true + } + } + GridLayout { + columns: 2 + rowSpacing: ScreenTools.defaultFontPixelHeight * 0.5 + columnSpacing: ScreenTools.defaultFontPixelHeight * 0.5 + visible: settingsHeader.checked + + FactCheckBox { + text: qsTr("Show All") + fact: wimaController.showAllMissionItems + Layout.fillWidth: true + } + + FactCheckBox { + text: qsTr("Show Current") + fact: wimaController.showCurrentMissionItems + Layout.fillWidth: true + } + + FactCheckBox { + text: qsTr("Reverse") + fact: wimaController.reverse + Layout.fillWidth: true + } + } // Grid + + SectionHeader{ + id: missionHeader + text: qsTr("Mission") + } + GridLayout { + columns: 2 + rowSpacing: ScreenTools.defaultFontPixelHeight * 0.5 + columnSpacing: ScreenTools.defaultFontPixelHeight * 0.5 + visible: missionHeader.checked + width: parent.width + + QGCLabel { + text: qsTr("Speed") + Layout.fillWidth: true + } + FactTextField { + fact: wimaController.flightSpeed + Layout.fillWidth: true + } + + QGCLabel { + text: qsTr("Altitude") + Layout.fillWidth: true + } + FactTextField { + fact: wimaController.altitude + Layout.fillWidth: true + } + + } + + GridLayout { + columns: 2 + rowSpacing: ScreenTools.defaultFontPixelHeight * 0.5 + columnSpacing: ScreenTools.defaultFontPixelHeight * 0.5 + visible: missionHeader.checked + width: parent.width + + // Buttons + QGCButton { + id: buttonPreviousMissionPhase + text: qsTr("Reverse") + onClicked: wimaController.previousPhase() + Layout.fillWidth: true + } + + QGCButton { + id: buttonNextMissionPhase + text: qsTr("Forward") + onClicked: wimaController.nextPhase() + Layout.fillWidth: true + } + + QGCButton { + id: buttonResetPhase + text: qsTr("Reset Phase") + onClicked: wimaController.resetPhase(); + Layout.columnSpan: 2 + Layout.fillWidth: true + } + } // Grid + + SectionHeader{ + id: vehicleHeader + text: qsTr("Vehicle") + } + GridLayout { + columns: 2 + rowSpacing: ScreenTools.defaultFontPixelHeight * 0.5 + columnSpacing: ScreenTools.defaultFontPixelHeight * 0.5 + visible: vehicleHeader.checked + width: parent.width + + QGCButton { + id: buttonUpload + text: qsTr("Upload") + onClicked: wimaController.uploadToVehicle() + Layout.fillWidth: true + } + + QGCButton { + id: buttonRemoveFromVehicle + text: qsTr("Remove") + onClicked: wimaController.removeFromVehicle() + Layout.fillWidth: true + } + + QGCButton { + id: buttonSmartRTL + text: qsTr("Smart RTL") + onClicked: initSmartRTL() + Layout.columnSpan: 2 + Layout.fillWidth: true + } + + + // progess bar + Rectangle { + id: progressBar + height: 4 + width: _controllerProgressPct * parent.width + color: qgcPal.colorGreen + visible: false + Layout.columnSpan: 2 + Layout.fillWidth: true + } + + QGCLabel { + id: uploadCompleteText + font.pointSize: ScreenTools.largeFontPointSize + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignHCenter + verticalAlignment: Text.AlignVCenter + text: "Done" + visible: false + Layout.fillWidth: true + } + } + + SectionHeader { + id: statsHeader + text: qsTr("Statistics") + } + GridLayout { + columns: 2 + rowSpacing: ScreenTools.defaultFontPixelHeight * 0.5 + anchors.topMargin: ScreenTools.defaultFontPixelHeight * 0.5 + visible: statsHeader.checked + + QGCLabel { + text: qsTr("Phase Length: ") + wrapMode: Text.WordWrap + Layout.fillWidth: true + } + QGCLabel { + text: wimaController.phaseDistance >= 0 ? wimaController.phaseDistance.toFixed(2) + " m": "" + wrapMode: Text.WordWrap + Layout.fillWidth: true + } + + QGCLabel { + text: qsTr("Phase Duration: ") + wrapMode: Text.WordWrap + Layout.fillWidth: true + } + QGCLabel { + text: wimaController.phaseDuration >= 0 ? getTime(wimaController.phaseDuration) : "" + wrapMode: Text.WordWrap + Layout.fillWidth: true + } + QGCLabel { + text: "" + Layout.columnSpan: 2 + } + } + } // settingsColumn + } // QGCFlickable } } diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index aa7c9ceeab13dbfa1dfd176aebf93459eeb9c226..a320207ed2ef77d57197cb6992581088531f2648 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -32,8 +32,8 @@ WimaController::WimaController(QObject *parent) , _flightSpeed (settingsGroup, _metaDataMap[flightSpeedName]) , _altitude (settingsGroup, _metaDataMap[altitudeName]) , _reverse (settingsGroup, _metaDataMap[reverseName]) + , _endWaypointIndex (0) , _startWaypointIndex (0) - , _lastMissionPhaseReached (false) , _uploadOverrideRequired (false) , _phaseDistance (-1) , _phaseDuration (-1) @@ -42,7 +42,7 @@ WimaController::WimaController(QObject *parent) , _executingSmartRTL (false) { - _nextPhaseStartWaypointIndex.setRawValue(int(1)); + //_nextPhaseStartWaypointIndex.setRawValue(int(1)); _showAllMissionItems.setRawValue(true); _showCurrentMissionItems.setRawValue(true); connect(&_overlapWaypoints, &Fact::rawValueChanged, this, &WimaController::updateNextWaypoint); @@ -50,6 +50,7 @@ WimaController::WimaController(QObject *parent) connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::calcNextPhase); connect(&_flightSpeed, &Fact::rawValueChanged, this, &WimaController::updateSpeed); connect(&_altitude, &Fact::rawValueChanged, this, &WimaController::updateAltitude); + connect(&_reverse, &Fact::rawValueChanged, this, &WimaController::reverseChangedHandler); // setup low battery handling connect(&_checkBatteryTimer, &QTimer::timeout, this, &WimaController::checkBatteryLevel); @@ -218,19 +219,35 @@ void WimaController::nextPhase() } void WimaController::previousPhase() -{ - if (_nextPhaseStartWaypointIndex.rawValue().toInt() > 0) { - _lastMissionPhaseReached = false; - _nextPhaseStartWaypointIndex.setRawValue(std::max(_startWaypointIndex - - _maxWaypointsPerPhase.rawValue().toInt() - + _overlapWaypoints.rawValue().toInt(), 1)); +{ + bool reverseBool = _reverse.rawValue().toBool(); + if (!reverseBool){ + int startIndex = _nextPhaseStartWaypointIndex.rawValue().toInt(); + if (startIndex > 0) { + _nextPhaseStartWaypointIndex.setRawValue(1+std::max(_startWaypointIndex + - _maxWaypointsPerPhase.rawValue().toInt() + + _overlapWaypoints.rawValue().toInt(), 0)); + } + } + else { + int startIndex = _nextPhaseStartWaypointIndex.rawValue().toInt(); + if (startIndex <= _missionItems.count()) { + _nextPhaseStartWaypointIndex.setRawValue(1+std::min(_startWaypointIndex + + _maxWaypointsPerPhase.rawValue().toInt() + - _overlapWaypoints.rawValue().toInt(), _missionItems.count()-1)); + } } } void WimaController::resetPhase() { - _lastMissionPhaseReached = false; - _nextPhaseStartWaypointIndex.setRawValue(int(1)); + bool reverseBool = _reverse.rawValue().toBool(); + if (!reverseBool) { + _nextPhaseStartWaypointIndex.setRawValue(int(1)); + } + else { + _nextPhaseStartWaypointIndex.setRawValue(_missionItems.count()); + } } bool WimaController::uploadToVehicle() @@ -450,7 +467,6 @@ bool WimaController::fetchContainerData() emit currentWaypointPathChanged(); _localPlanDataValid = false; - _lastMissionPhaseReached = false; if (_container == nullptr) { qWarning("WimaController::fetchContainerData(): No container assigned!"); @@ -536,7 +552,8 @@ bool WimaController::fetchContainerData() // set _nextPhaseStartWaypointIndex to 1 disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::calcNextPhase); - _nextPhaseStartWaypointIndex.setRawValue(int(1)); + bool reverse = _reverse.rawValue().toBool(); + _nextPhaseStartWaypointIndex.setRawValue(reverse? _missionItems.count() : int(1)); connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::calcNextPhase); if(!calcNextPhase()) @@ -554,46 +571,51 @@ bool WimaController::fetchContainerData() bool WimaController::calcNextPhase() { - if (_missionItems.count() < 1 || _lastMissionPhaseReached) + if (_missionItems.count() < 1) { + _startWaypointIndex = 0; + _endWaypointIndex = 0; return false; + } _currentMissionItems.clearAndDeleteContents(); _currentWaypointPath.clear(); emit currentMissionItemsChanged(); emit currentWaypointPathChanged(); - bool reverseBool = _reverse.rawValue().toBool(); // Reverses the phase direction. Phases go from high to low waypoint numbers, if true. - _startWaypointIndex = _nextPhaseStartWaypointIndex.rawValue().toInt()-1; - if (!reverseBool) { - if (_startWaypointIndex > _missionItems.count()-1) + bool reverse = _reverse.rawValue().toBool(); // Reverses the phase direction. Phases go from high to low waypoint numbers, if true. + int startIndex = _nextPhaseStartWaypointIndex.rawValue().toInt()-1; + if (!reverse) { + if (startIndex > _missionItems.count()-1) return false; } else { - if (_startWaypointIndex < 0) + if (startIndex < 0) return false; - } + } + _startWaypointIndex = startIndex; - int maxWaypointsPerPhaseInt = _maxWaypointsPerPhase.rawValue().toInt(); + int maxWaypointsPerPhase = _maxWaypointsPerPhase.rawValue().toInt(); // determine end waypoint index - if (!reverseBool) { - _endWaypointIndex = std::min(_startWaypointIndex + maxWaypointsPerPhaseInt - 1, _missionItems.count()-1); + bool lastMissionPhaseReached = false; + if (!reverse) { + _endWaypointIndex = std::min(_startWaypointIndex + maxWaypointsPerPhase - 1, _missionItems.count()-1); if (_endWaypointIndex == _missionItems.count() - 1) - _lastMissionPhaseReached = true; + lastMissionPhaseReached = true; } else { - _endWaypointIndex = std::max(_startWaypointIndex - maxWaypointsPerPhaseInt + 1, 0); + _endWaypointIndex = std::max(_startWaypointIndex - maxWaypointsPerPhase + 1, 0); if (_endWaypointIndex == 0) - _lastMissionPhaseReached = true; + lastMissionPhaseReached = true; } // extract waypoints QList geoCoordinateList; // list with potential waypoints (from _missionItems), for _currentMissionItems - if (!reverseBool) { + if (!reverse) { if (!extractCoordinateList(_missionItems, geoCoordinateList, _startWaypointIndex, _endWaypointIndex)) { qWarning("WimaController::calcNextPhase(): error on waypoint extraction."); return false; @@ -614,9 +636,9 @@ bool WimaController::calcNextPhase() // set start waypoint index for next phase - if (!_lastMissionPhaseReached) { + if (!lastMissionPhaseReached) { disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::calcNextPhase); - if (!reverseBool) { + if (!reverse) { int untruncated = std::max(_endWaypointIndex + 1 - _overlapWaypoints.rawValue().toInt(), 0); int truncated = std::min(untruncated , _missionItems.count()-1); _nextPhaseStartWaypointIndex.setRawValue(truncated + 1); @@ -747,7 +769,6 @@ void WimaController::updateNextWaypoint() void WimaController::recalcCurrentPhase() { - _lastMissionPhaseReached = false; disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::calcNextPhase); _nextPhaseStartWaypointIndex.setRawValue(_startWaypointIndex + 1); connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::calcNextPhase); @@ -845,6 +866,15 @@ void WimaController::enableDisableLowBatteryHandling(QVariant enable) } } +void WimaController::reverseChangedHandler() +{ + disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::calcNextPhase); + _nextPhaseStartWaypointIndex.setRawValue(_endWaypointIndex+1); + connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::calcNextPhase); + + calcNextPhase(); +} + void WimaController::_setPhaseDistance(double distance) { if (!qFuzzyCompare(distance, _phaseDistance)) { @@ -996,7 +1026,7 @@ void WimaController::_setVehicleHasLowBattery(bool batteryLow) bool WimaController::_executeSmartRTL(QString &errorSring) { - Q_UNUSED(errorSring); + Q_UNUSED(errorSring) _executingSmartRTL = true; connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::smartRTLCleanUp); forceUploadToVehicle(); diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 2d4a061d8a07bddbb232e537f08a6389be47b4d6..152f0d73b020cddf745b80c6d906221a02b3dbab 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -177,6 +177,7 @@ private slots: void checkBatteryLevel (void); void smartRTLCleanUp (bool flying); // cleans up after successfull smart RTL void enableDisableLowBatteryHandling (QVariant enable); + void reverseChangedHandler (); private: void _setPhaseDistance(double distance); @@ -224,7 +225,6 @@ private: // (which is not part of the return path) of _currentMissionItem int _startWaypointIndex; // indes of the mission item stored in _missionItems defining the first element // (which is not part of the arrival path) of _currentMissionItem - bool _lastMissionPhaseReached; bool _uploadOverrideRequired; // Is set to true if uploadToVehicle() did not suceed because the vehicle is not inside the service area. // The user can override the upload lock with a slider, this will reset this variable to false. double _phaseDistance; // the lenth in meters of the current phase diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index beac49f86cec440f2725a61fb40dbea4b5666140..2f06d60b23ef888ac437ce6ad25ea3455a6ebad3 100644 --- a/src/Wima/WimaPlaner.cc +++ b/src/Wima/WimaPlaner.cc @@ -708,7 +708,11 @@ WimaPlanData WimaPlaner::toPlanData() // convert mission items to mavlink commands QObject deleteObject; // used to automatically delete content of rgMissionItems after this function call QList rgMissionItems; - MissionController::convertToMissionItems(_missionController->visualItems(), rgMissionItems, &deleteObject); + QmlObjectListModel *visualItems = _missionController->visualItems(); + QmlObjectListModel visualItemsToCopy; + for (int i = _arrivalPathLength+1; i < visualItems->count()-_returnPathLength; i++) + visualItemsToCopy.append(visualItems->get(i)); + MissionController::convertToMissionItems(&visualItemsToCopy, rgMissionItems, &deleteObject); // store mavlink commands planData.append(rgMissionItems);