autopilot/drone_controller/az_drone_controller.cpp

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

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

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


AzDroneController::AzDroneController(AzMission &azMission, QObject *parent)
    : QObject(parent)
    , mMavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}} // TODO!! Autopilot or CompanionComputer?
    , mDroneState(AZ_DRONE_STATE_DISCONNECTED)
    , mMissionController(nullptr)
    , mAzMission(azMission)
{
    mFirstPosition.relative_altitude_m = -10000;

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

    isCopterType = QCoreApplication::arguments().at(2) == "quadcopter";
}


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 {
        cout << "[CONTROLLER] MAVSDK::add_any_connection() failed. Reason: " << result << endl;
        return false;
    }
}


bool AzDroneController::stateGetSystem(void)
{
    vector<shared_ptr<System>> systems = mMavsdk.systems();

    if (systems.size() == 0) {
        return false;
    }

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

    if (autopilot.has_value()) {
        mSystem = autopilot.value();
        return true;
    }

    return false;
}


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

    const auto setRateResult = mTelemetry->set_rate_position(1); // 1 Hz
    if (setRateResult != Telemetry::Result::Success) {
        cout << "[CONTROLLER] Setting telemetry rate failed: " << setRateResult << endl;
        return false;
    }

    // Subscripe to position and heading updates. Updated comes from different MAVSDK thread. Send position
    // or heading 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);
    connect(this, &AzDroneController::newHeading, this, &AzDroneController::newHeadingSlot, Qt::QueuedConnection);
    mTelemetry->subscribe_position([this](Telemetry::Position position) { emit newPosition(position); });
    mTelemetry->subscribe_heading([this](Telemetry::Heading heading) { emit newHeading(heading.heading_deg); });

    return true;
}


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

    return mAction == nullptr ? false : true;
}


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

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

    return result;
}


bool AzDroneController::stateUploadMission(void)
{
    if (mMissionController == nullptr) {
        mMissionController = new AzMissionController(*mSystem.get(), mAzMission, this);
    }

    return mMissionController->uploadMission();
}


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

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


bool AzDroneController::stateTakeoff(void)
{
    mAction->set_takeoff_altitude(AZ_RELATIVE_FLY_ALTITUDE);
    Action::Result result = mAction->takeoff();
    cout << "[CONTROLLER] MAVSDK::Action::takeoff() failed. Reason: " << result << endl;
    return result == Action::Result::Success;
}


bool AzDroneController::stateFlyMission(void)
{
    // Drone never reaches the target altitude with ArduPilot. This seems
    // to be a bug in MAVSDK/ArduPilot, because this doesn't happen with PX4.
    // Added extra check to AzDroneController::stateFlyMission() to not start the
    // mission before 90% of AZ_RELATIVE_FLY_ALTITUDE is reached.
    if (isCopterType && mCurrentPosition.relative_altitude_m < AZ_RELATIVE_FLY_ALTITUDE * 0.90) {
        cout << "[CONTROLLER] 90% of the takeoff altitide is not reached yet." << endl;
        return false;
    }

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

    return mMissionController->startMission();
}


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

    if (result != Action::Result::Success) {
        cout << "[CONTROLLER] MAVSDK::Action::land() failed. Reason: " << result << endl;
    }
}


void AzDroneController::droneStateMachineSlot(void)
{
    using MethodPtr = bool (AzDroneController::*)();

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

    static const MethodInfo stateMethods[]
        = {{AZ_DRONE_STATE_DISCONNECTED, &AzDroneController::stateConnect, "Connecting to autopilot", 1000},
           {AZ_DRONE_STATE_CONNECTED, &AzDroneController::stateGetSystem, "Getting MAVSDK system", 1000},
           {AZ_DRONE_STATE_GOT_SYSTEM, &AzDroneController::stateGetTelemetryModule, "Getting telemetry module", 1000},
           {AZ_DRONE_STATE_GOT_TELEMETRY_MODULE, &AzDroneController::stateGetActionModule, "Getting action module", 1000},
           {AZ_DRONE_STATE_GOT_ACTION_MODULE, &AzDroneController::stateHealthOk, "Drone health OK", 1000},
           {AZ_DRONE_STATE_HEALTH_OK, &AzDroneController::stateArm, "Arming the drone", 1000},
           {AZ_DRONE_STATE_ARMED, &AzDroneController::stateTakeoff, "Takeoff the drone", 10000},
           {AZ_DRONE_STATE_TAKE_OFF, &AzDroneController::stateFlyMission, "Starting the mission", 1000},
           {AZ_DRONE_STATE_FLY_MISSION, nullptr, "Mission started", 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) {
                cout << "[CONTROLLER] Waiting signals from the mission controller." << endl;
                return;
            }

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

            if (result) {
                cout << "[CONTROLLER] " << stateMethods[i].description << " succeeded." << endl;
                mDroneState = stateMethods[i + 1].currentState;
            }
            else {
                cout << "[CONTROLLER] " << stateMethods[i].description << " failed. Trying again." << endl;
            }

            delayedStateCallSlot(stateMethods[i].nextStateDelay);

            break;
        }
    }
}


void AzDroneController::newPositionSlot(Telemetry::Position position)
{
    /*
    static Telemetry::Position previousPosition;

    // Only print position if it has changed.
    if (position == previousPosition) {
        return;
    }


    previousPosition = position;
    */
    //cout << "[CONTROLLER] GPS position: " << position.latitude_deg << ", " << position.longitude_deg
    //     << " Altitudes: " << position.absolute_altitude_m << ", " << position.relative_altitude_m << endl;

    // 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) {
        cout << "[CONTROLLER] First altitude " << mFirstPosition.relative_altitude_m << endl;
        mFirstPosition = position;
    }

    mCurrentPosition = position;

    // Send new position to the mission controller.
    if (mDroneState == AZ_DRONE_STATE_FLY_MISSION && mMissionController) {
        mMissionController->newPosition(position);
    }
}


void AzDroneController::newHeadingSlot(double heading)
{
    (void)heading;
    //cout << "[CONTROLLER] Heading: " << heading << endl;
}


void AzDroneController::missionIndexChangedSlot(int currentIndex, int totalIndexes)
{
    cout << "[CONTROLLER] missionIndexChanged() " << currentIndex << "/" << totalIndexes << endl;
}


void AzDroneController::newHealthInfoSlot(Telemetry::Health health)
{
    cout << "[CONTROLLER] newHealthInfo:" << endl;
    cout << "[CONTROLLER]   Gyro calibration:  " << (health.is_gyrometer_calibration_ok ? "OK" : "FAILED") << endl;
    cout << "[CONTROLLER]   Accel calibration: " << (health.is_accelerometer_calibration_ok ? "OK" : "FAILED") << endl;
    cout << "[CONTROLLER]   Mag calibration:   " << (health.is_magnetometer_calibration_ok ? "OK" : "FAILED") << endl;
    cout << "[CONTROLLER]   Local position:    " << (health.is_local_position_ok ? "OK" : "FAILED") << endl;
    cout << "[CONTROLLER]   Global position:   " << (health.is_global_position_ok ? "OK" : "FAILED") << endl;
    cout << "[CONTROLLER]   Home position:     " << (health.is_home_position_ok ? "OK" : "FAILED") << endl;

    // Stop printing healt information if everything in healt is OK.
    if (health.is_gyrometer_calibration_ok &&
        health.is_accelerometer_calibration_ok &&
        health.is_magnetometer_calibration_ok &&
        health.is_local_position_ok &&
        health.is_global_position_ok &&
        health.is_home_position_ok) {

        disconnect(this, &AzDroneController::newHealthInfo, this, &AzDroneController::newHealthInfoSlot);
    }
}