autopilot/src/az_mission_controller.cpp

#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 {
            std::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 {
        std::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 &coordinate = mMission.getReturnPoint();
        float distance
            = AzUtils::distance(pos.latitude_deg, pos.longitude_deg, coordinate.latitude, coordinate.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 (distance <= 0.01) {
            qDebug() << "AzMissionController::newPosition() target reached. Continuing to the next item.";
            flyToNextMissionItem();
        }
    }
}