autopilot/drone_controller/az_mission_controller.cpp

#include <QCoreApplication>
#include <QDebug>

#include <mavsdk/plugins/telemetry/telemetry.h>

#include "az_mission.h"
#include "az_mission_controller.h"
#include "az_utils.h"

AzMissionController::AzMissionController(AzMission &mission, QObject *parent)
    : QObject(parent)
    , mMission(mission)
    , mAction(nullptr)
    , mMissionStarted(false)
    , mFlyingToReturnPoint(false)
    , mAbsoluteAltitude(-10000)
{}

void AzMissionController::setAction(Action *action)
{
    mAction = action;
}

bool AzMissionController::startMissions(float absoluteAltitude)
{
    mCurrentMissionIndex = -1;
    mMissionStarted      = true;
    mAbsoluteAltitude    = absoluteAltitude; // TODO!! Use altitudes from the JSON file.
    return flyToNextMissionItem();
}

void AzMissionController::stopMissions(void)
{
    // TODO!! Needs to be improved. At least send signal to AzDroneController.
    mMissionStarted = false;
}

bool AzMissionController::flyToNextMissionItem(void)
{
    mCurrentMissionIndex++;

    if (mCurrentMissionIndex == (int) mMission.getActionPoints().size()) {
        mFlyingToReturnPoint = true;
        qDebug() << "AzMissionController::flyToNextMissionItem() All action points handled.";
        qDebug() << "AzMissionController::flyToNextMissionItem() Flying to the JSON return point.";
        const AzCoordinate &coordinate = mMission.getReturnPoint();
        Action::Result result
            = mAction->goto_location(coordinate.latitude, coordinate.longitude, mAbsoluteAltitude, 0.0f);

        if (result == Action::Result::Success) {
            return true;
        }
        else {
            cerr << "mAction->goto_location() failed. Reason: " << result << endl;
            return false;
        }
    }

    qDebug() << "AzMissionController::flyToNextMissionItem() flying to the item" << mCurrentMissionIndex + 1 << "/"
             << mMission.getActionPoints().size();

    const AzCoordinate &coordinate = mMission.getActionPoints().at(mCurrentMissionIndex).getPoint();
    qDebug() << "AzMissionController::flyToNextMissionItem() navigating to point" << coordinate.latitude
             << coordinate.longitude;

    Action::Result result = mAction->goto_location(coordinate.latitude, coordinate.longitude, mAbsoluteAltitude, 0.0f);

    // TODO!! Check return value and print warnings and errors.
    if (result == Action::Result::Success) {
        emit missionProgressChanged(mCurrentMissionIndex + 1, mMission.getActionPoints().size());
        return true;
    }
    else {
        cerr << "mAction->goto_location() failed. Reason: " << result << endl;
        return false;
    }
}

void AzMissionController::newPosition(Telemetry::Position pos)
{
    if (mMissionStarted == false) {
        return;
    }

    if (mFlyingToReturnPoint == true) {
        const AzCoordinate &point = mMission.getReturnPoint();
        float distance = AzUtils::distance(pos.latitude_deg, pos.longitude_deg, point.latitude, point.longitude);

        qDebug() << "AzMissionController::newPosition() distance to return point:" << distance << " km.";

        if (distance <= 0.01) {
            qDebug() << "AzMissionController::newPosition() return point reached. Mission finished.";
            mMissionStarted      = false;
            mFlyingToReturnPoint = false;
            emit finished();
        }
    }
    else {
        const AzCoordinate &target = mMission.getActionPoints().at(mCurrentMissionIndex).getPoint();
        float distance = AzUtils::distance(pos.latitude_deg, pos.longitude_deg, target.latitude, target.longitude);

        qDebug() << "AzMissionController::newPosition() distance to target:" << distance << "km.";

        // TODO!! In final application we need to use the camera to find the target.
        if (QCoreApplication::arguments().contains("quadcopter") && distance <= 0.01) {
            qDebug() << "AzMissionController::newPosition() target reached. Continuing to the next item.";
            flyToNextMissionItem();
        }
        else if (QCoreApplication::arguments().contains("plane")) {
            if (mPlaneCirclingTime.isValid() == false && distance <= 0.1) {
                qDebug() << "AzMissionController::newPosition() target reached. Starting circling.";
                mPlaneCirclingTime.restart();
            }
            else if (mPlaneCirclingTime.elapsed() > 35000) {
                qDebug() << "AzMissionController::newPosition() target reached. Ending circling.";
                mPlaneCirclingTime.invalidate();
                flyToNextMissionItem();
            }
        }
    }
}