autopilot/src/az_drone_controller.cpp

#include <QCoreApplication>
#include <QDebug>
#include <QMetaProperty>
#include <QThread>
#include <QTimer>

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

#include "az_config.h"
#include "az_drone_controller.h"

AzDroneController::AzDroneController(AzMission &mission, QObject *parent)
    : QObject(parent)
    , mMavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}
    , // TODO!! Autopilot or CompanionComputer?
    mDroneState(AZ_DRONE_STATE_DISCONNECTED)
{
    qDebug() << "AzDroneController::AzDroneController() Thread ID: " << QThread::currentThreadId();
    mFirstPosition.relative_altitude_m = -10000;

    mMissionController = new AzMissionController(mission, this);

    // Mission progress from the AzMissionController. Slot will be used to find the targets later.
    connect(
        mMissionController,
        &AzMissionController::missionProgressChanged,
        this,
        &AzDroneController::missionIndexChangedSlot);

    // Mission controller signals end of the missions. This will be used to fly to the return point in JSON.
    connect(mMissionController, &AzMissionController::finished, this, &AzDroneController::missionFinishedSlot);

    // Healt info update from MAVSDK.
    connect(this, &AzDroneController::newHealthInfo, this, &AzDroneController::newHealthInfoSlot, Qt::QueuedConnection);
}

void AzDroneController::start()
{
    // Must wait that main event loop is launched in main()
    delayedStateCallSlot(0);
}

void AzDroneController::delayedStateCallSlot(int ms)
{
    // MAVSDK examples use blocking sleep() calls for timeouts.
    // QTimer provides non-blocking state machine operations.
    QTimer::singleShot(ms, this, &AzDroneController::droneStateMachineSlot);
}

// Connects to the flight controller based on AZ_CONNECTION_XXX defines in AzConfig.
// Serial port connections is enabled if command line arguments contains "serial"
// parameter. Otherwise UDP connection is used.
bool AzDroneController::stateConnect(void)
{
    ConnectionResult result;

    if (QCoreApplication::arguments().contains("serial")) {
        result = mMavsdk.add_any_connection(AZ_CONNECTION_SERIAL);
    }
    else {
        result = mMavsdk.add_any_connection(AZ_CONNECTION_UDP);
    }

    if (result == ConnectionResult::Success) {
        return true;
    }
    else {
        std::cerr << "MAVSDK::add_any_connection() failed. Reason: " << result << endl;
        return false;
    }
}

bool AzDroneController::stateAutopilot(void)
{
    std::vector<std::shared_ptr<System>> systems = mMavsdk.systems();
    qDebug() << "AzDroneController::stateAutopilot() Found" << systems.size() << "systems";

    if (systems.size() == 0) {
        std::cerr << "No system found." << std::endl;
        return false;
    }

    std::optional<std::shared_ptr<System>> autopilot = mMavsdk.first_autopilot(AZ_GET_AUTOPILOT_TIMEOUT);

    if (autopilot.has_value()) {
        mSystem = autopilot.value();
        return true;
    }
    else {
        qCritical() << "MAVSDK::first_autopilot() failed";
        return false;
    }
}

bool AzDroneController::stateTelemetryModule(void)
{
    if ((mTelemetry = new Telemetry(mSystem)) == nullptr) {
        return false;
    }

    const auto setRateResult = mTelemetry->set_rate_position(1); // 1 Hz
    if (setRateResult != Telemetry::Result::Success) {
        qCritical() << "Setting rate failed: " << (int) setRateResult;
        return false;
    }

    // Subscripe to position updates. Updated comes from different MAVSDK thread. Send position
    // as signal to this class (Qt::QueuedConnection) so that it's handled in the main thread.
    qRegisterMetaType<Telemetry::Position>("Telemetry::Position");
    connect(this, &AzDroneController::newPosition, this, &AzDroneController::newPositionSlot, Qt::QueuedConnection);
    mTelemetry->subscribe_position([this](Telemetry::Position position) { emit newPosition(position); });

    // TODO!! This doesn't work. Check how ArduPilot sets home position.
    Telemetry::Position home = mTelemetry->home();
    qDebug() << "Home: lat:" << home.latitude_deg << "lon:" << home.longitude_deg
             << "altitude abs:" << home.absolute_altitude_m << "altitude rel:" << home.relative_altitude_m;

    return true;
}

bool AzDroneController::stateActionModule(void)
{
    mAction = new Action(mSystem);

    if (mAction != nullptr) {
        mMissionController->setAction(mAction);
        return true;
    }
    else {
        qWarning() << "Creating new MAVSDK::Action failed";
        return false;
    }
}

bool AzDroneController::stateReadyForArming(void)
{
    bool result = mTelemetry->health_all_ok();

    if (result == false) {
        mTelemetry->subscribe_health([this](Telemetry::Health health) { emit newHealthInfo(health); });
    }

    return result;
}

bool AzDroneController::stateArm(void)
{
    Action::Result result = mAction->arm();

    if (result == Action::Result::Success) {
        return true;
    }
    else {
        std::cerr << "MAVSDK::Action::arm() failed. Reason: " << result << endl;
        return false;
    }
}

bool AzDroneController::stateTakeoff(void)
{
    // TODO!! Drone never reaches the target altitude with ArduPilot. Investigate and fix.
    mAction->set_takeoff_altitude(50);

    // TODO!! Check return value and print warnings and errors.
    Action::Result result = mAction->takeoff();
    return result == Action::Result::Success;
}

bool AzDroneController::stateFlyMission(void)
{
    // TODO!! Check with the team about fly altitude. Is altitudes in JSON file absolute or relative?
    float flight_altitude_abs = AZ_RELATIVE_FLY_ALTITUDE + mFirstPosition.absolute_altitude_m;
    return mMissionController->startMissions(flight_altitude_abs);
}

void AzDroneController::missionFinishedSlot(void)
{
    // TODO!! Maybe use Telemetry::subscribe_landed_state() later to get the state of the landing.
    qDebug() << "AzDroneController::missionFinishedSlot() Mission finished. Landing the drone.";
    Action::Result result = mAction->land();

    if (result != Action::Result::Success) {
        std::cerr << "mAction->land() failed. Reason: " << result << endl;
    }
}

void AzDroneController::droneStateMachineSlot(void)
{
    static const QString states[] = {
        "DISCONNECTED",
        "CONNECTED",
        "AUTOPILOT",
        "TELEMETRY_MODULE",
        "ACTION_MODULE",
        "READY_FOR_ARMING",
        "ARMED",
        "TAKE_OFF",
        "FLY_MISSION",
        "LAND",
    };

    qDebug() << "";
    qDebug() << "AzDroneController::droneStateMachineSlot() Current state:" << states[(int) mDroneState];

    using MethodPtr = bool (AzDroneController::*)();

    struct MethodInfo
    {
        AzDroneState currentState;
        MethodPtr nextStateMethodPtr;
        QString description;
        int nextStateDelay;
    };

    static const MethodInfo stateMethods[]
        = {{AZ_DRONE_STATE_DISCONNECTED, &AzDroneController::stateConnect, "stateConnect()", 1000},
           {AZ_DRONE_STATE_CONNECTED, &AzDroneController::stateAutopilot, "stateAutopilot()", 1000},
           {AZ_DRONE_STATE_AUTOPILOT, &AzDroneController::stateTelemetryModule, "stateTelemetryModule()", 1000},
           {AZ_DRONE_STATE_TELEMETRY_MODULE, &AzDroneController::stateActionModule, "stateActionModule()", 1000},
           {AZ_DRONE_STATE_ACTION_MODULE, &AzDroneController::stateReadyForArming, "stateReadyForArming()", 1000},
           {AZ_DRONE_STATE_READY_FOR_ARMING, &AzDroneController::stateArm, "stateArm()", 1000},
           {AZ_DRONE_STATE_ARMED, &AzDroneController::stateTakeoff, "stateTakeoff()", 10000},
           {AZ_DRONE_STATE_TAKE_OFF, &AzDroneController::stateFlyMission, "stateFlyMission()", 1000},
           {AZ_DRONE_STATE_FLY_MISSION, nullptr, "No function to call. AzMissionControl running", 0}};

    // Iterate through states. If the state function fails, it's repeated until it succeeds.
    for (uint i = 0; i < sizeof(stateMethods) / sizeof(MethodInfo); i++) {
        if (stateMethods[i].currentState == mDroneState) {
            if (mDroneState == AZ_DRONE_STATE_FLY_MISSION) {
                qDebug() << "AzMissionContoroller running. Waiting signals from it.";
                return;
            }

            bool result = (this->*stateMethods[i].nextStateMethodPtr)();

            if (result) {
                qDebug() << "AzDroneController::droneStateMachineSlot()" << stateMethods[i].description << "succeeded";

                mDroneState = stateMethods[i + 1].currentState;

                // Quadcopter needs to iterate all states - plane will skip arming and takeoff
                if (mDroneState == AZ_DRONE_STATE_READY_FOR_ARMING && QCoreApplication::arguments().contains("plane")) {
                    mDroneState = AZ_DRONE_STATE_TAKE_OFF;
                    qDebug() << "AzDroneController::droneStateMachineSlot() Skipping arming and takeoff for planes.";
                }
                else {
                    qDebug() << "AzDroneController::droneStateMachineSlot() New state:" << states[(int) mDroneState];
                }
            }
            else {
                qDebug() << "AzDroneController::droneStateMachineSlot()" << stateMethods[i].description
                         << "failed. Trying again.";
            }

            delayedStateCallSlot(stateMethods[i].nextStateDelay);

            break;
        }
    }
}

void AzDroneController::newPositionSlot(Telemetry::Position position)
{
    qDebug() << "AzDroneController::newPositionSlot()" << position.latitude_deg << position.longitude_deg;

    // Save first position. It will be used later to set altitude for missions.
    // TODO!! Probably we want to use rangefinder or at least barometer with altitude from the map later.
    if (mFirstPosition.relative_altitude_m < -1000) {
        qDebug() << "AzDroneController::newPositionSlot()"
                 << "First altitude" << mFirstPosition.relative_altitude_m << "lat:" << position.latitude_deg
                 << "lon:" << position.longitude_deg;
        mFirstPosition = position;
    }

    // Send new position to the mission controller.
    if (mDroneState == AZ_DRONE_STATE_FLY_MISSION) {
        mMissionController->newPosition(position);
    }
    else {
        qDebug() << "AzDroneController::newPositionSlot() altitude rel:" << position.relative_altitude_m
                 << "altitude abs:" << position.absolute_altitude_m;
    }
}

void AzDroneController::missionIndexChangedSlot(int currentIndex, int totalIndexes)
{
    qDebug() << "AzDroneController::missionIndexChanged()" << currentIndex << "/" << totalIndexes;
}

void AzDroneController::newHealthInfoSlot(Telemetry::Health health)
{
    qDebug() << "AzDroneController::newHealthInfoSlot()";
    qDebug() << "  Gyro calibration:  " << (health.is_gyrometer_calibration_ok ? "ok" : "not ok");
    qDebug() << "  Accel calibration: " << (health.is_accelerometer_calibration_ok ? "ok" : "not ok");
    qDebug() << "  Mag calibration:   " << (health.is_magnetometer_calibration_ok ? "ok" : "not ok");
    qDebug() << "  Local position:    " << (health.is_local_position_ok ? "ok" : "not ok");
    qDebug() << "  Global position:   " << (health.is_global_position_ok ? "ok" : "not ok");
    qDebug() << "  Home position:     " << (health.is_home_position_ok ? "ok" : "not ok");

    // Not optimal, but disconnect signal immeaditely after getting the first update.
    disconnect(this, &AzDroneController::newHealthInfo, this, &AzDroneController::newHealthInfoSlot);
}