WimaController.cc

            }
        }
        else {
            _setVehicleHasLowBattery(false);
            _lowBatteryHandlingTriggered = false;
        }

    }
}

void WimaController::smartRTLCleanUp(bool flying)
{

    if ( !flying) { // vehicle has landed
        if (_executingSmartRTL) {
            _executingSmartRTL = false;
            _loadCurrentMissionItemsFromBuffer();
            _showAllMissionItems.setRawValue(true);
            disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::smartRTLCleanUp);
        }
    }
}

void WimaController::enableDisableLowBatteryHandling(QVariant enable)
{
    if (enable.toBool()) {
        _checkBatteryTimer.start();
    } else {
        _checkBatteryTimer.stop();
    }
}

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)) {
        _phaseDistance = distance;

        emit phaseDistanceChanged();
    }
}

void WimaController::_setPhaseDuration(double duration)
{
    if (!qFuzzyCompare(duration, _phaseDuration)) {
        _phaseDuration = duration;

        emit phaseDurationChanged();
    }
}

bool WimaController::_checkSmartRTLPreCondition(QString &errorString)
{
    if (!_localPlanDataValid) {
        errorString.append(tr("No WiMA data available. Please define at least a measurement and a service area."));
        return false;
    }
    Vehicle *managerVehicle = masterController()->managerVehicle();
   if (!managerVehicle->flying()) {
        errorString.append(tr("Vehicle is not flying. Smart RTL not available."));
        return false;
   }

   return true;
}

bool WimaController::_calcReturnPath(QString &errorSring)
{
    // it is assumed that checkSmartRTLPreCondition() was called first and true was returned


    Vehicle *managerVehicle = masterController()->managerVehicle();

    QGeoCoordinate currentVehiclePosition = managerVehicle->coordinate();
    // check if vehicle inside _joinedArea, this statement is not inside checkSmartRTLPreCondition() because during checkSmartRTLPreCondition() vehicle is not paused yet
    if (!_joinedArea.containsCoordinate(currentVehiclePosition)) {
        errorSring.append(tr("Vehicle not inside joined area. Action not supported."));
        return false;
    }

    // calculate return path
    QVector<QGeoCoordinate> returnPath;
    calcShortestPath(currentVehiclePosition, _takeoffLandPostion, returnPath);
    // successful?
    if (returnPath.isEmpty()) {
        errorSring.append(tr("Not able to calculate return path."));
        return false;
    }
    // qWarning() << "returnPath.size()" << returnPath.size();

    _saveCurrentMissionItemsToBuffer();

    // create Mission Items
    removeFromVehicle();
    QmlObjectListModel* missionControllerVisuals = _missionController->visualItems();

    // set homeposition of settingsItem
    MissionSettingsItem* settingsItem = missionControllerVisuals->value<MissionSettingsItem *>(0);
    if (settingsItem == nullptr) {
        qWarning("WimaController: nullptr");
        return false;
    }
    settingsItem->setCoordinate(_takeoffLandPostion);

    // copy from returnPath to _missionController
    QGeoCoordinate speedItemCoordinate = returnPath.first();
    for (auto coordinate : returnPath) {
        _missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count());
    }
    //qWarning() << "missionControllerVisuals->count()" << missionControllerVisuals->count();

    // create speed item
    int speedItemIndex = 1;
    _missionController->insertSimpleMissionItem(speedItemCoordinate, speedItemIndex);
    SimpleMissionItem *speedItem = missionControllerVisuals->value<SimpleMissionItem*>(speedItemIndex);
    if (speedItem == nullptr) {
        qWarning("WimaController: nullptr");
        return false;
    }
    speedItem->setCoordinate(speedItemCoordinate);
    speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);
    speedItem->missionItem().setParam2(_flightSpeed.rawValue().toDouble());

    // set second item command to ordinary waypoint (is takeoff)
    SimpleMissionItem *secondItem = missionControllerVisuals->value<SimpleMissionItem*>(2);
    if (secondItem == nullptr) {
        qWarning("WimaController: nullptr");
        return false;
    }
    secondItem->setCoordinate(speedItemCoordinate);
    secondItem->setCommand(MAV_CMD_NAV_WAYPOINT);


    // set land command for last mission item
    SimpleMissionItem *landItem = missionControllerVisuals->value<SimpleMissionItem*>(missionControllerVisuals->count()-1);
    if (landItem == nullptr) {
        qWarning("WimaController: nullptr");
        return false;
    }
    _missionController->setLandCommand(*landItem);

    // copy to _currentMissionItems
    //qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count();
    for ( int i = 1; i < missionControllerVisuals->count(); i++) {
        SimpleMissionItem *visualItem     = missionControllerVisuals->value<SimpleMissionItem*>(i);
        if (visualItem == nullptr) {
            qWarning("WimaController: Nullptr at SimpleMissionItem!");
            _currentMissionItems.clear();
            return false;
        }

        SimpleMissionItem *visualItemCopy = new SimpleMissionItem(*visualItem, true, this);
        _currentMissionItems.append(visualItemCopy);
    }
    //qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count();

    _setPhaseDistance(_missionController->missionDistance());
    _setPhaseDuration(_missionController->missionDistance()/_flightSpeed.rawValue().toDouble());
    _missionController->removeAll(); // remove items from _missionController, will be added on upload
    updateAltitude();

    updateCurrentPath();
    emit currentMissionItemsChanged();


    //qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count();
    _showAllMissionItems.setRawValue(false);
    managerVehicle->trajectoryPoints()->clear();

    emit uploadAndExecuteConfirmRequired();
    return true;
}

void WimaController::_setVehicleHasLowBattery(bool batteryLow)
{
    if (_vehicleHasLowBattery != batteryLow) {
        _vehicleHasLowBattery = batteryLow;

        emit vehicleHasLowBatteryChanged();
    }
}

bool WimaController::_executeSmartRTL(QString &errorSring)
{
    Q_UNUSED(errorSring)
    _executingSmartRTL = true;
    connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::smartRTLCleanUp);
    forceUploadToVehicle();
    masterController()->managerVehicle()->startMission();

    return true;
}

void WimaController::_loadCurrentMissionItemsFromBuffer()
{
    _currentMissionItems.clear();
    int numItems = _missionItemsBuffer.count();
    for (int i = 0; i < numItems; i++)
        _currentMissionItems.append(_missionItemsBuffer.removeAt(0));

    updateCurrentPath();
}


void WimaController::_saveCurrentMissionItemsToBuffer()
{
    _missionItemsBuffer.clear();
    int numCurrentMissionItems = _currentMissionItems.count();
    for (int i = 0; i < numCurrentMissionItems; i++)
        _missionItemsBuffer.append(_currentMissionItems.removeAt(0));
}