WimaController.cc

void WimaController::_enableDisableLowBatteryHandling(QVariant enable)
{
    if (enable.toBool()) {
        _eventTimer.start();
    } else {
        _eventTimer.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;
    }

    if (!_joinedArea.containsCoordinate(managerVehicle->coordinate())) {
         errorString.append(tr("Vehicle not inside save area. 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();
    _phaseDistanceBuffer = _phaseDistance;
    _phaseDurationBuffer = _phaseDuration;


    // 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->setCommand(MAV_CMD_DO_CHANGE_SPEED);
    speedItem->setCoordinate(speedItemCoordinate);
    speedItem->missionItem().setParam2(_arrivalReturnSpeed.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();

    double dist = 0;
    double time = 0;
    if (!_missionController->distanceTimeToMissionEnd(dist, time, 1, false))
        qWarning("WimaController::calcNextPhase: distanceTimeToMissionEnd returned false!");
    _setPhaseDistance(dist);
    _setPhaseDuration(time);
    _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();

    return true;
}

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

        emit vehicleHasLowBatteryChanged();
    }
}

void WimaController::_initSmartRTL()
{
    QString errorString;
    static int attemptCounter = 0;
    attemptCounter++;

    if (_checkSmartRTLPreCondition(errorString) == true) {
        _masterController->managerVehicle()->pauseVehicle();
        connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp);
        if (_calcReturnPath(errorString)) {
            _executingSmartRTL = true;
            attemptCounter = 0;

            switch(_srtlReason) {
                case BatteryLow:
                    emit returnBatteryLowConfirmRequired();
                break;
                case UserRequest:
                    emit returnUserRequestConfirmRequired();
                break;
                default:
                    qWarning("\nWimaController::_initSmartRTL: default case reached!");
            }

            return;
        }
    }
    if (attemptCounter > SMART_RTL_MAX_ATTEMPTS) { // try 3 times, somtimes vehicle is outside joined area
        errorString.append(tr("Smart RTL: No success after maximum number of attempts."));
        qgcApp()->showMessage(errorString);
        attemptCounter = 0;
    } else {
        _smartRTLAttemptTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this, &WimaController::_initSmartRTL);
    }
}

void WimaController::_executeSmartRTL()
{
    forceUploadToVehicle();
    masterController()->managerVehicle()->startMission();
}

void WimaController::_setSnakeConnectionStatus(WimaController::SnakeConnectionStatus status)
{
    if (_snakeConnectionStatus != status) {
        _snakeConnectionStatus = status;
        emit snakeConnectionStatusChanged();
    }
}

void WimaController::_setSnakeCalcInProgress(bool inProgress)
{
    if (_snakeCalcInProgress != inProgress){
        _snakeCalcInProgress = inProgress;
        emit snakeCalcInProgressChanged();
    }
}

bool WimaController::_verifyScenarioDefined()
{
    _scenarioDefinedBool = _scenario.defined(_snakeTileWidth.rawValue().toDouble(), _snakeTileHeight.rawValue().toDouble(), _snakeMinTileArea.rawValue().toDouble());
    return _scenarioDefinedBool;
}

bool WimaController::_verifyScenarioDefinedWithErrorMessage()
{
    bool value = _verifyScenarioDefined();
    if (!value){
        QString errorString;
        for (auto c : _scenario.errorString)
            errorString.push_back(c);
        qgcApp()->showMessage(errorString);
    }
    return value;
}

void WimaController::_snakeStoreWorkerResults()
{
    _missionItems.clearAndDeleteContents();
    const WorkerResult_t &r = _snakeWorker.getResult();
    _setSnakeCalcInProgress(false);
    if (!r.success) {
        qgcApp()->showMessage(r.errorMessage);
        return;
    }

    // create Mission items from r.waypoints
    long n = r.waypoints.size() - r.returnPathIdx.size() - r.arrivalPathIdx.size() + 2;
    assert(n >= 1);
    QVector<MissionItem> missionItems;
    missionItems.reserve(int(n));

    // Remove all items from mission controller.
    _missionController->removeAll();
    QmlObjectListModel* missionControllerVisualItems = _missionController->visualItems();

    // Create QVector<QGeoCoordinate> containing all waypoints;
    unsigned long startIdx = r.arrivalPathIdx.last();
    unsigned  long endIdx = r.returnPathIdx.first();
    QVector<QGeoCoordinate> waypoints;
    for (unsigned long i = startIdx; i <= endIdx; ++i) {
        QGeoCoordinate wp{r.waypoints[int(i)].value<QGeoCoordinate>()};
        waypoints.append(wp);
    }

    // create SimpleMissionItem by using _missionController
    long insertIdx = missionControllerVisualItems->count();
    for (auto wp : waypoints)
        _missionController->insertSimpleMissionItem(wp, insertIdx++);
    SimpleMissionItem *takeOffItem = missionControllerVisualItems->value<SimpleMissionItem*>(1);
    if (takeOffItem == nullptr) {
        qWarning("WimaController::_snakeStoreWorkerResults(): Nullptr at SimpleMissionItem!");
        return;
    }
    takeOffItem->setCommand(MAV_CMD_NAV_WAYPOINT);
    takeOffItem->setCoordinate(waypoints[0]);

    // copy mission items from _missionController to _missionItems
    cout.precision(10);
    for ( int i = 1; i < missionControllerVisualItems->count(); i++) {
        SimpleMissionItem *visualItem     = qobject_cast<SimpleMissionItem *>((*missionControllerVisualItems)[i]);
        if (visualItem == nullptr) {
            qWarning("WimaController::_snakeStoreWorkerResults(): Nullptr at SimpleMissionItem!");
            return;
        }
        SimpleMissionItem *visualItemCopy = new SimpleMissionItem(*visualItem, true, this);
        _missionItems.append(visualItemCopy);
    }

    _updateWaypointPath();

    // set _nextPhaseStartWaypointIndex to 1
    disconnect(&_nextPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
    bool reverse = _reverse.rawValue().toBool();
    _nextPhaseStartWaypointIndex.setRawValue(reverse? _missionItems.count() : int(1));
    connect(&_nextPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);

    _calcNextPhase();
    emit missionItemsChanged();
}

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));
}