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 &mission, QObject *parent)
    : QObject(parent)
    , mMavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}} // TODO!! Autopilot or CompanionComputer?
    , mDroneState(AZ_DRONE_STATE_DISCONNECTED)
    , mMissionRaw(NULL)
    , mMissionItemSeqNum(0)
{
    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);

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


MissionRaw::MissionItem AzDroneController::makeRawMissionItem(
    float latitude_deg1e7,
    float longitude_deg1e7,
    int32_t altitude_m,
    float do_photo,
    MAV_FRAME frame,
    MAV_CMD command,
    float p2,
    float p3)
{
    (void)p2;
    (void)p3;

    MissionRaw::MissionItem new_item{};
    new_item.seq = mMissionItemSeqNum;
    new_item.frame = static_cast<uint32_t>(frame);
    new_item.command = static_cast<uint32_t>(command);
    new_item.param1 = do_photo;
    new_item.x = latitude_deg1e7 * 1e7;
    new_item.y = longitude_deg1e7 * 1e7;
    new_item.z = altitude_m;
    new_item.mission_type = MAV_MISSION_TYPE_MISSION;
    new_item.autocontinue = 1;

    if (mMissionItemSeqNum == 1) {
        new_item.current = 1;
    }

    mMissionItemSeqNum++;

    return new_item;
}


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

    if (mAction != nullptr) {
        mMissionController->setAction(mAction);
        return true;
    }

    return false;
}


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)
{
    cout << "[CONTROLLER] stateUploadMission() Setting raw mission starts" << endl;

    mMissionItemSeqNum = 0;

    if (mMissionRaw == NULL) {
        mMissionRaw = new MissionRaw(*mSystem);
    }

    mMissionRaw->clear_mission();

    if (mMissionRaw == NULL) {
        mMissionRaw = new MissionRaw(*mSystem);
    }

    auto clearResult = mMissionRaw->clear_mission();
    if (clearResult != MissionRaw::Result::Success) {
        std::cout << "[CONTROLLER] stateUploadMission() Clearing mMissionRaw failed" << std::endl;
        return false;
    }

    auto downloadResult = mMissionRaw->download_mission();
    if (downloadResult.first != MissionRaw::Result::Success) {
        std::cout << "[CONTROLLER] stateUploadMission() Downloading mission failed" << std::endl;
        return false;
    }

    // first point in case of ardupilot is always home
    auto missionPlan = downloadResult.second;
    MissionRaw::MissionItem homePoint = missionPlan[0];
    missionPlan.clear();

    // going relative alt mission so we dont care about altitude
    auto latDeg = homePoint.x * 1e-7;
    auto lonDeg = homePoint.y * 1e-7;

    // in case of ardupilot we want to set lat lon to 0, to use current position as takeoff position
    missionPlan.push_back(makeRawMissionItem(
        0, // lat
        0, // lon
        50.0,
        0,
        MAV_FRAME_GLOBAL_RELATIVE_ALT,
        MAV_CMD_NAV_TAKEOFF));

    // setup speed during mission execution
    missionPlan.push_back(makeRawMissionItem(
        0, 0, 0, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_DO_CHANGE_SPEED, 9.35f, -1.0f));

    missionPlan.push_back(makeRawMissionItem(
        latDeg + 0.001,
        lonDeg + 0.001,
        50.00,
        0,
        MAV_FRAME_GLOBAL_TERRAIN_ALT,
        MAV_CMD_NAV_WAYPOINT));

    missionPlan.push_back(makeRawMissionItem(
        latDeg + 0.001,
        lonDeg - 0.001,
        50.00,
        0,
        MAV_FRAME_GLOBAL_TERRAIN_ALT,
        MAV_CMD_NAV_WAYPOINT));

    missionPlan.push_back(makeRawMissionItem(
        latDeg - 0.001, lonDeg, 50.00, 0, MAV_FRAME_GLOBAL_TERRAIN_ALT, MAV_CMD_NAV_WAYPOINT));

    missionPlan.push_back(makeRawMissionItem(
        latDeg, lonDeg, 50.00, 0, MAV_FRAME_GLOBAL_TERRAIN_ALT, MAV_CMD_NAV_LAND));

    missionPlan.push_back(
        makeRawMissionItem(0, 0, 0, 0, MAV_FRAME_GLOBAL_INT, MAV_CMD_NAV_RETURN_TO_LAUNCH));

    for (const auto& item : missionPlan) {
        std::cout << "[CONTROLLER] stateUploadMission() seq: " << (int)item.seq << '\n';
    }

    auto uploadResult = mMissionRaw->upload_mission(missionPlan);
    if (uploadResult != MissionRaw::Result::Success) {
        std::cout << "[CONTROLLER] stateUploadMission() upload failed. Result: " << uploadResult << std::endl;
        return false;
    }

    mMissionRaw->set_current_mission_item(0);
    cout << "[CONTROLLER] stateUploadMission() Setting raw mission ends" << endl;

    return true;
}


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

    // start mission, this set autopilot auto mode.
    // ignore result, we dont care for now
    MissionRaw::Result startResult = mMissionRaw->start_mission();
    cout << "Start mission result:" << startResult << endl;
    cout << "AzDroneController::stateFlyMission() ends\n";

    return startResult == MissionRaw::Result::Success;
}


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