123

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<QGeoCoordinate> 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();

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

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<MissionItem*> 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);