Changed directory structure and renamed applications

- autopilot -> drone_controller
- rtsp_ai_player -> ai_controller
- added top level qmake project file
- updated documentation
- moved small demo applications from tmp/ to misc/

drone_controller/.clang-format

@@ -0,0 +1,105 @@
+Language:        Cpp
+SeparateDefinitionBlocks: Always
+EmptyLineBeforeAccessModifier: LogicalBlock
+MaxEmptyLinesToKeep: 2
+AccessModifierOffset: -4
+AlignAfterOpenBracket: AlwaysBreak
+AlignConsecutiveAssignments: true
+AlignConsecutiveDeclarations: None
+AlignEscapedNewlines: DontAlign
+AlignOperands: true
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: true
+AllowShortBlocksOnASingleLine: false
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: Inline
+AllowShortIfStatementsOnASingleLine: true
+AllowShortLoopsOnASingleLine: false
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: false
+AlwaysBreakTemplateDeclarations: Yes
+BinPackArguments: false
+BinPackParameters: false
+BraceWrapping:
+  AfterClass:      true
+  AfterControlStatement: false
+  AfterEnum:       false
+  AfterFunction:   true
+  AfterNamespace:  false
+  AfterObjCDeclaration: false
+  AfterStruct:     true
+  AfterUnion:      false
+  BeforeCatch:     false
+  BeforeElse:      true
+  IndentBraces:    false
+  SplitEmptyFunction: false
+  SplitEmptyRecord: false
+  SplitEmptyNamespace: false
+BreakBeforeBinaryOperators: All
+BreakBeforeBraces: Custom
+BreakBeforeInheritanceComma: false
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializersBeforeComma: false
+BreakConstructorInitializers: BeforeComma
+BreakAfterJavaFieldAnnotations: false
+BreakStringLiterals: true
+ColumnLimit:    120
+CommentPragmas:  '^ IWYU pragma:'
+CompactNamespaces: false
+ConstructorInitializerAllOnOneLineOrOnePerLine: false
+ConstructorInitializerIndentWidth: 4
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+DerivePointerAlignment: false
+DisableFormat:   false
+ExperimentalAutoDetectBinPacking: false
+FixNamespaceComments: true
+ForEachMacros:
+  - forever # avoids { wrapped to next line
+  - foreach
+  - Q_FOREACH
+  - BOOST_FOREACH
+IncludeCategories:
+  - Regex:           '^<Q.*'
+    Priority:        200
+IncludeIsMainRegex: '(Test)?$'
+IndentCaseLabels: false
+IndentWidth:     4
+IndentWrappedFunctionNames: false
+InsertBraces: false
+JavaScriptQuotes: Leave
+JavaScriptWrapImports: true
+KeepEmptyLinesAtTheStartOfBlocks: false
+# Do not add QT_BEGIN_NAMESPACE/QT_END_NAMESPACE as this will indent lines in between.
+MacroBlockBegin: ""
+MacroBlockEnd:   ""
+NamespaceIndentation: None
+ObjCBlockIndentWidth: 4
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakAssignment: 88
+PenaltyBreakBeforeFirstCallParameter: 300
+PenaltyBreakComment: 500
+PenaltyBreakFirstLessLess: 400
+PenaltyBreakString: 600
+PenaltyExcessCharacter: 50
+PenaltyReturnTypeOnItsOwnLine: 300
+PointerAlignment: Right
+ReflowComments:  false
+SortIncludes: CaseSensitive
+SortUsingDeclarations: true
+SpaceAfterCStyleCast: true
+SpaceAfterTemplateKeyword: false
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeParens: ControlStatements
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 1
+SpacesInAngles:  Never
+SpacesInContainerLiterals: false
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+Standard: c++17
+TabWidth:        4
+UseTab:          Never
+

drone_controller/.gitignore

@@ -0,0 +1,13 @@
+.cmake/
+.qtc/
+.qtc_clangd/
+CMakeCache.txt
+CMakeCache.txt.prev
+CMakeFiles/
+CMakeLists.txt.user
+Makefile
+Testing/
+azaion
+cmake_install.cmake
+qtcsettings.cmake
+

drone_controller/az_action_point.cpp

@@ -0,0 +1,68 @@
+#include "az_action_point.h"
+
+AzActionPoint::AzActionPoint(
+    const AzCoordinate &point, int height, AzActionPointType actionPointType, uint actionSpesific)
+    : mPoint(point)
+    , mHeight(height)
+    , mType(actionPointType)
+    , mActionSpesific(actionSpesific)
+{}
+
+AzCoordinate AzActionPoint::getPoint(void) const
+{
+    return mPoint;
+}
+
+int AzActionPoint::getHeight(void) const
+{
+    return mHeight;
+}
+
+AzActionPointType AzActionPoint::getType(void) const
+{
+    return mType;
+}
+
+string AzActionPoint::getTypeStr(void) const
+{
+    static const string typeStrs[] = {"None", "Waypoint", "Search", "Return"};
+    return typeStrs[mType];
+}
+
+string AzActionPoint::getActionSpecificStr(void) const
+{
+    if (isTank() == true && isArtillery() == true) {
+        return "tank or artillery";
+    }
+    else if (isTank() == true) {
+        return "tank";
+    }
+    else if (isArtillery() == true) {
+        return "artillery";
+    }
+    else {
+        return "none";
+    }
+}
+
+bool AzActionPoint::isTank(void) const
+{
+    return mActionSpesific & AZ_ACTION_SPECIFIC_TANK;
+}
+
+bool AzActionPoint::isArtillery(void) const
+{
+    return mActionSpesific & AZ_ACTION_SPECIFIC_ARTILLERY;
+}
+
+ostream &operator<<(ostream &os, const AzActionPoint &obj)
+{
+    os << "Point: " << obj.mPoint << endl;
+    os << "Height: " << obj.mHeight << endl;
+    os << "Type: " << obj.getTypeStr() << endl;
+
+    // TODO!! This is braindead.
+    os << "Action specific type: " + obj.getActionSpecificStr() << endl;
+
+    return os;
+}

drone_controller/az_action_point.h

@@ -0,0 +1,44 @@
+#pragma once
+
+#include "az_coordinate.h"
+
+enum AzActionPointType {
+    AZ_ACTION_POINT_TYPE_NONE     = 0,
+    AZ_ACTION_POINT_TYPE_WAYPOINT = 1,
+    AZ_ACTION_POINT_TYPE_SEARCH   = 2,
+    AZ_ACTION_POINT_TYPE_RETURN   = 3
+};
+
+enum AzActionSpecific {
+    AZ_ACTION_SPECIFIC_NONE      = 1 << 0,
+    AZ_ACTION_SPECIFIC_TANK      = 1 << 1,
+    AZ_ACTION_SPECIFIC_ARTILLERY = 1 << 2,
+};
+
+class AzActionPoint
+{
+public:
+    AzActionPoint(const AzCoordinate &point, int height, AzActionPointType actionPointType, uint actionSpesific);
+
+    AzCoordinate getPoint(void) const;
+
+    int getHeight(void) const;
+
+    AzActionPointType getType(void) const;
+
+    string getTypeStr(void) const;
+
+    bool isTank(void) const;
+
+    bool isArtillery(void) const;
+
+    string getActionSpecificStr(void) const;
+
+    friend ostream &operator<<(ostream &os, const AzActionPoint &obj);
+
+private:
+    AzCoordinate mPoint;
+    int mHeight;
+    AzActionPointType mType;
+    uint mActionSpesific;
+};

drone_controller/az_config.h

@@ -0,0 +1,7 @@
+#pragma once
+
+// Please check the port name! Also baudrates must match or devices can't communicate with each other.
+const char *AZ_CONNECTION_SERIAL     = "serial:///dev/ttyS0:115200";
+const char *AZ_CONNECTION_UDP        = "udp://:14550";
+const int AZ_RELATIVE_FLY_ALTITUDE   = 50;
+const float AZ_GET_AUTOPILOT_TIMEOUT = 3.0;

drone_controller/az_coordinate.cpp

@@ -0,0 +1,12 @@
+#include "az_coordinate.h"
+
+AzCoordinate::AzCoordinate(double lat, double lon)
+    : latitude(lat)
+    , longitude(lon)
+{}
+
+ostream &operator<<(ostream &os, const AzCoordinate &obj)
+{
+    os << "(" << obj.latitude << ", " << obj.longitude << ")";
+    return os;
+}

drone_controller/az_coordinate.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#include <ostream>
+
+using namespace std;
+
+const double INVALID_LATITUDE  = -100000;
+const double INVALID_LONGITUDE = -100000;
+
+class AzCoordinate
+{
+public:
+    double latitude;
+    double longitude;
+
+    AzCoordinate(double lat, double lon);
+
+    friend ostream &operator<<(ostream &os, const AzCoordinate &obj);
+};

drone_controller/az_drone_controller.cpp

@@ -0,0 +1,291 @@
+#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)
+{
+    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 {
+        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 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); });
+
+    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::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)
+{
+    // 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.
+
+    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)
+{
+    if (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);
+}
+
+
+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)
+{
+    cout << "[CONTROLLER] GPS position " << position.latitude_deg << ", " << position.longitude_deg << 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::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);
+    }
+}

drone_controller/az_drone_controller.h

@@ -0,0 +1,91 @@
+#pragma once
+
+#include <memory.h>
+#include <QObject>
+
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/action/action.h>
+#include <mavsdk/plugins/telemetry/telemetry.h>
+
+#include "az_mission.h"
+#include "az_mission_controller.h"
+
+using namespace mavsdk;
+
+typedef enum {
+    AZ_DRONE_STATE_DISCONNECTED,
+    AZ_DRONE_STATE_CONNECTED,
+    AZ_DRONE_STATE_GOT_SYSTEM,
+    AZ_DRONE_STATE_GOT_TELEMETRY_MODULE,
+    AZ_DRONE_STATE_GOT_ACTION_MODULE,
+    AZ_DRONE_STATE_HEALTH_OK,
+    AZ_DRONE_STATE_ARMED,
+    AZ_DRONE_STATE_TAKE_OFF,
+    AZ_DRONE_STATE_AUTO_MODE_ACTIVATED,
+    AZ_DRONE_STATE_FLY_MISSION,
+    AZ_DRONE_STATE_LAND,
+    AZ_DRONE_STATE_END,
+} AzDroneState;
+
+class AzDroneController : public QObject
+{
+    Q_OBJECT
+public:
+    explicit AzDroneController(AzMission &mission, QObject *parent = nullptr);
+
+    // Starts the autopilot.
+    void start();
+
+protected:
+    // Specify the methods that will be called one at a time to initialise the drone and fly the mission.
+    bool stateConnect(void);
+    bool stateGetSystem(void);
+    bool stateGetTelemetryModule(void);
+    bool stateGetActionModule(void);
+    bool stateHealthOk(void);
+    bool stateArm(void);
+    bool stateTakeoff(void);
+    bool stateFlyMission(void);
+
+    // Slots that are called by the emitted Qt signals.
+protected slots:
+    // Launches a state machine that progressively initialises the drone without blocking.
+    virtual void droneStateMachineSlot(void);
+
+    // ArduPilot seems to require some delay between calls. This method uses QTimer internally
+    // to call each new state method with some delay without blocking the main thread.
+    void delayedStateCallSlot(int ms);
+
+    // Slot that is called when the newPosition() signal below is emitted.
+    void newPositionSlot(Telemetry::Position);
+
+    // A mission file contains several action points. This is called
+    // when the action point is reached. Indexing starts at 1.
+    void missionIndexChangedSlot(int currentIndex, int totalIndexes);
+
+    // Called at the end of the mission. Lands and disarms the drone.
+    void missionFinishedSlot(void);
+
+    // Gets Telemetry::Health information in prearming.
+    void newHealthInfoSlot(Telemetry::Health health);
+
+signals:
+    // Signal is emitted when Ardupilot sends a new position from the
+    // MAVSDK thread. Signal goes through the main event loop and is
+    // captured in the main thread to avoid threading issues.
+    void newPosition(Telemetry::Position);
+
+    // If Telemetry::health_all_ok() fails, then autopilot subscripes for the healt updates to
+    // see exactly what is wrong. This signal is emitted to catch updates in the main thread.
+    void newHealthInfo(Telemetry::Health);
+
+protected:
+    Mavsdk mMavsdk;
+    AzDroneState mDroneState;
+    shared_ptr<System> mSystem;
+    Telemetry *mTelemetry;
+    Action *mAction;
+    Telemetry::Position mFirstPosition;
+    Telemetry::Position mCurrentPosition;
+    AzMissionController *mMissionController;
+};

drone_controller/az_drone_controller_plane.cpp

@@ -0,0 +1,64 @@
+#include <QCoreApplication>
+#include <QDebug>
+#include "az_drone_controller_plane.h"
+
+AzDroneControllerPlane::AzDroneControllerPlane(AzMission &mission, QObject *parent) :
+    AzDroneController(mission, parent)
+{}
+
+
+bool AzDroneControllerPlane::stateWaitAutoSwitch(void)
+{
+    Telemetry::FlightMode flight_mode = mTelemetry->flight_mode();
+    cout << "[CONTROLLER] Current flight mode: " << flight_mode << endl;
+    return flight_mode == Telemetry::FlightMode::Mission;
+}
+
+
+void AzDroneControllerPlane::droneStateMachineSlot(void)
+{
+    using MethodPtr = bool (AzDroneControllerPlane::*)();
+
+    struct MethodInfo
+    {
+        AzDroneState currentState;
+        MethodPtr nextStateMethodPtr;
+        string description;
+        int nextStateDelay;
+    };
+
+    // Plane type will pass arming and taking off states and wait for auto mode from the RC controller instead.
+    static const MethodInfo stateMethods[]
+        = {{AZ_DRONE_STATE_DISCONNECTED, &AzDroneControllerPlane::stateConnect, "Connecting to autopilot", 1000},
+           {AZ_DRONE_STATE_CONNECTED, &AzDroneControllerPlane::stateGetSystem, "Getting MAVSDK system", 1000},
+           {AZ_DRONE_STATE_GOT_SYSTEM, &AzDroneControllerPlane::stateGetTelemetryModule, "Getting telemetry module", 1000},
+           {AZ_DRONE_STATE_GOT_TELEMETRY_MODULE, &AzDroneControllerPlane::stateGetActionModule, "Getting action module", 1000},
+           {AZ_DRONE_STATE_GOT_ACTION_MODULE, &AzDroneControllerPlane::stateHealthOk, "Drone health OK", 1000},
+           {AZ_DRONE_STATE_HEALTH_OK, &AzDroneControllerPlane::stateWaitAutoSwitch, "User switched to AUTO mode", 1000},
+           {AZ_DRONE_STATE_AUTO_MODE_ACTIVATED, &AzDroneControllerPlane::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;
+        }
+    }
+}

drone_controller/az_drone_controller_plane.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#include "az_drone_controller.h"
+#include <QObject>
+
+class AzDroneControllerPlane : public AzDroneController
+{
+    Q_OBJECT
+public:
+    explicit AzDroneControllerPlane(AzMission &mission, QObject *parent = nullptr);
+
+protected:
+    void droneStateMachineSlot(void) override;
+    bool stateWaitAutoSwitch(void);
+};

drone_controller/az_mission.cpp

@@ -0,0 +1,145 @@
+#include <filesystem>
+#include <iostream>
+#include <string>
+
+#include "../3rd/rapidjson/include/rapidjson/document.h"
+#include "../3rd/rapidjson/include/rapidjson/error/en.h"
+#include "../3rd/rapidjson/include/rapidjson/filereadstream.h"
+
+#include "az_mission.h"
+
+using namespace rapidjson;
+using namespace std;
+
+AzMission::AzMission(string filename)
+    : mFilename(filename)
+    , mOperationalHeight(INVALID_HEIGHT)
+    , mReturnPoint(AzCoordinate(INVALID_LATITUDE, INVALID_LONGITUDE))
+{
+    if (filesystem::exists(mFilename) == false) {
+        cerr << "JSON file " << mFilename << " doesn't exists." << endl;
+        exit(1);
+    }
+
+    parse();
+}
+
+void AzMission::parse(void)
+{
+    FILE *fp = fopen(mFilename.c_str(), "r");
+    char readBuffer[65536];
+    FileReadStream is(fp, readBuffer, sizeof(readBuffer));
+
+    Document document;
+    document.ParseStream(is);
+    fclose(fp);
+
+    if (document.HasParseError()) {
+        cerr << "JSON parse error: " << GetParseError_En(document.GetParseError()) << endl;
+        exit(1);
+    }
+
+    // Parse operational height
+    int operational_height = document["operational_height"].GetInt();
+    if (operational_height > 0) {
+        mOperationalHeight = operational_height;
+    }
+
+    // Parse geofences
+    const Value &geofences = document["geofences"];
+    const Value &polygons  = geofences["polygons"];
+    for (SizeType i = 0; i < polygons.Size(); i++) {
+        AzGeofence geofence;
+        const Value &polygon = polygons[i];
+        const Value &points  = polygon["points"];
+        for (SizeType j = 0; j < points.Size(); j++) {
+            AzCoordinate coord(points[j][0].GetDouble(), points[j][1].GetDouble());
+            geofence.coordinates.push_back(coord);
+        }
+        string fence_type = polygon["fence_type"].GetString();
+        if (fence_type == "INCLUSION") {
+            geofence.type = AZ_GEOFENCE_TYPE_INCLUSION;
+        }
+        else if (fence_type == "INCLUSION") {
+            geofence.type = AZ_GEOFENCE_TYPE_EXCLUSION;
+        }
+
+        mGeofences.push_back(geofence);
+    }
+
+    // Parse action points
+    const Value &action_points = document["action_points"];
+    for (SizeType i = 0; i < action_points.Size(); i++) {
+        double lat = action_points[i]["point"][0].GetDouble();
+        double lon = action_points[i]["point"][1].GetDouble();
+        AzCoordinate point(lat, lon);
+
+        int height = action_points[i]["height"].GetInt();
+
+        string action_enum                  = action_points[i]["action_enum"].GetString();
+        AzActionPointType action_point_type = AZ_ACTION_POINT_TYPE_NONE;
+        if (action_enum == "waypoint") {
+            action_point_type = AZ_ACTION_POINT_TYPE_WAYPOINT;
+        }
+        else if (action_enum == "search") {
+            action_point_type = AZ_ACTION_POINT_TYPE_SEARCH;
+        }
+        else if (action_enum == "return") {
+            action_point_type = AZ_ACTION_POINT_TYPE_RETURN;
+        }
+
+        uint actionSpefific = 0;
+        if (action_points[i].HasMember("action_specific")) {
+            const Value &targets = action_points[i]["action_specific"]["targets"];
+            for (SizeType j = 0; j < targets.Size(); j++) {
+                if (targets[j].GetString() == string("tank")) {
+                    actionSpefific |= AZ_ACTION_SPECIFIC_TANK;
+                }
+                else if (targets[j].GetString() == string("artillery")) {
+                    actionSpefific |= AZ_ACTION_SPECIFIC_ARTILLERY;
+                }
+            }
+        }
+
+        AzActionPoint actionPoint(point, height, action_point_type, actionSpefific);
+        mActionPoints.push_back(actionPoint);
+    }
+
+    // Parse return_point
+    mReturnPoint = AzCoordinate(document["return_point"][0].GetDouble(), document["return_point"][1].GetDouble());
+}
+
+const vector<AzGeofence> &AzMission::getGeofences(void) const
+{
+    return mGeofences;
+}
+
+const vector<AzActionPoint> &AzMission::getActionPoints(void) const
+{
+    return mActionPoints;
+}
+
+int AzMission::getOperationalHeight(void) const
+{
+    return mOperationalHeight;
+}
+
+AzCoordinate AzMission::getReturnPoint(void) const
+{
+    return mReturnPoint;
+}
+
+ostream &operator<<(ostream &os, const AzMission &obj)
+{
+    os << "\nMission defination:" << endl;
+    os << "----------------------" << endl;
+    os << "Filename: " << obj.mFilename << endl;
+    os << "Operational height: " << obj.mOperationalHeight << endl;
+    os << "Return point: " << obj.mReturnPoint << endl;
+
+    for (size_t i = 0; i < obj.mActionPoints.size(); i++) {
+        os << "\nAction point[" << i << "]:" << endl;
+        os << obj.mActionPoints[i];
+    }
+    return os;
+}

drone_controller/az_mission.h

@@ -0,0 +1,46 @@
+#pragma once
+
+#include <iostream>
+#include <string>
+#include <vector>
+
+#include "az_action_point.h"
+#include "az_coordinate.h"
+
+using namespace std;
+
+const double INVALID_HEIGHT = -100000;
+
+enum AzGeofenceType { AZ_GEOFENCE_TYPE_NONE = 0, AZ_GEOFENCE_TYPE_INCLUSION = 1, AZ_GEOFENCE_TYPE_EXCLUSION = 2 };
+
+class AzGeofence
+{
+public:
+    vector<AzCoordinate> coordinates;
+    AzGeofenceType type;
+};
+
+class AzMission
+{
+public:
+    AzMission(string filename);
+
+    const vector<AzGeofence> &getGeofences(void) const;
+
+    const vector<AzActionPoint> &getActionPoints(void) const;
+
+    int getOperationalHeight(void) const;
+
+    AzCoordinate getReturnPoint(void) const;
+
+    friend ostream &operator<<(ostream &os, const AzMission &obj);
+
+private:
+    void parse(void);
+
+    string mFilename;
+    vector<AzGeofence> mGeofences;
+    vector<AzActionPoint> mActionPoints;
+    int mOperationalHeight;
+    AzCoordinate mReturnPoint;
+};

drone_controller/az_mission_controller.cpp

@@ -0,0 +1,121 @@
+#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();
+            }
+        }
+    }
+}

drone_controller/az_mission_controller.h

@@ -0,0 +1,39 @@
+#pragma once
+
+#include <QObject>
+#include <QElapsedTimer>
+
+#include <mavsdk/plugins/telemetry/telemetry.h>
+
+#include "az_mission.h"
+#include "plugins/action/action.h"
+
+using namespace mavsdk;
+
+class AzMissionController : public QObject
+{
+    Q_OBJECT
+public:
+    explicit AzMissionController(AzMission &mission, QObject *parent = nullptr);
+    void setAction(Action *action);
+    bool startMissions(float absoluteAltitude);
+    void stopMissions(void);
+
+public slots:
+    void newPosition(Telemetry::Position position);
+
+signals:
+    void missionProgressChanged(int currentIndex, int totalIndexes);
+    void finished(void);
+
+private:
+    bool flyToNextMissionItem(void);
+
+    AzMission &mMission;
+    Action *mAction;
+    int mCurrentMissionIndex;
+    bool mMissionStarted;
+    bool mFlyingToReturnPoint;
+    float mAbsoluteAltitude;
+    QElapsedTimer mPlaneCirclingTime;
+};

drone_controller/az_utils.cpp

@@ -0,0 +1,21 @@
+#include "az_utils.h"
+#include <math.h>
+
+AzUtils::AzUtils() {}
+
+#define EARTH_RADIUS 6371.0 // Earth radius in kilometers
+
+double degrees_to_radians(double degrees)
+{
+    return degrees * M_PI / 180.0;
+}
+
+double AzUtils::distance(double lat1, double lon1, double lat2, double lon2)
+{
+    double dlat = degrees_to_radians(lat2 - lat1);
+    double dlon = degrees_to_radians(lon2 - lon1);
+    double a    = sin(dlat / 2) * sin(dlat / 2)
+               + cos(degrees_to_radians(lat1)) * cos(degrees_to_radians(lat2)) * sin(dlon / 2) * sin(dlon / 2);
+    double c = 2 * atan2(sqrt(a), sqrt(1 - a));
+    return EARTH_RADIUS * c;
+}

drone_controller/az_utils.h

@@ -0,0 +1,9 @@
+#pragma once
+
+class AzUtils
+{
+public:
+    AzUtils();
+
+    static double distance(double lat1, double lon1, double lat2, double lon2);
+};

drone_controller/drone_controller.pro

@@ -0,0 +1,33 @@
+QT = core
+CONFIG += cmdline
+
+# Reduce compiler warnings from the ArduPilot headers
+QMAKE_CXXFLAGS += -Wno-address-of-packed-member -std=gnu++1z
+
+# MAVSDK pkg-config file is garbage. Add dependency manually
+QMAKE_RPATHDIR += /usr/local/lib
+INCLUDEPATH += /usr/local/include/mavsdk
+LIBS += /usr/local/lib/libmavsdk.so
+
+SOURCES += \
+        az_action_point.cpp \
+        az_coordinate.cpp \
+        az_drone_controller.cpp \
+        az_drone_controller_plane.cpp \
+        az_mission.cpp \
+        az_mission_controller.cpp \
+        az_utils.cpp \
+        main.cpp
+
+target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+HEADERS += \
+    az_action_point.h \
+    az_config.h \
+    az_coordinate.h \
+    az_drone_controller.h \
+    az_drone_controller_plane.h \
+    az_mission.h \
+    az_mission_controller.h \
+    az_utils.h

drone_controller/main.cpp

@@ -0,0 +1,57 @@
+#include <ctime>
+
+#include <QCoreApplication>
+#include <QDebug>
+#include <QFile>
+
+#include "az_drone_controller.h"
+#include "az_drone_controller_plane.h"
+#include "az_mission.h"
+
+int main(int argc, char *argv[])
+{
+    // Add current data and time for easier log handling.
+    std::cout << "\n#################################################\n";
+    std::time_t currentTime = std::time(nullptr);
+    std::tm* localTime = std::localtime(&currentTime);
+    std::cout << "Starting autopilot program at " << std::put_time(localTime, "%Y-%m-%d %H:%M:%S") << std::endl;
+    std::cout << "#################################################\n";
+
+    // This is needed to have main event loop and signal-slot events in the AzDroneController.
+    QCoreApplication a(argc, argv);
+
+    if (a.arguments().size() != 4) {
+        qCritical() << "\nProgram needs three command line parameters.";
+        qCritical() << "\tFirst argument:  mission JSON file.";
+        qCritical() << "\tSecond argument: \"quadcopter\" or \"plane\".";
+        qCritical() << "\tThird argument:  \"udp\" or \"serial\" for connection type.";
+        qCritical() << "\tFor example ./autopilot mission.json plane udp";
+        return 1;
+    }
+
+    if (QFile::exists(a.arguments().at(1)) == false) {
+        qCritical() << "\nMission file doesn't exist";
+        return 1;
+    }
+
+    if (a.arguments().at(2) != "plane" && a.arguments().at(2) != "quadcopter") {
+        qCritical() << "\nPass \"quadcopter\" or \"plane\" for drone type as second argument";
+        return 1;
+    }
+
+    if (a.arguments().at(3) != "udp" && a.arguments().at(3) != "serial") {
+        qCritical() << "Pass \"udp\" or \"serial\" for connection type as third argument";
+        return 1;
+    }
+
+    // Reads mission from the JSON file which is given as command line option.
+    AzMission mission(argv[1]);
+    cout << mission;
+
+    // Launch a drone controller using the MAVSDK for ArduPilot-based flight controllers.
+    bool isPlane = a.arguments().at(2) == "plane";
+    AzDroneController *controller = isPlane ? new AzDroneControllerPlane(mission) : new AzDroneController(mission);
+    controller->start();
+
+    return a.exec();
+}

drone_controller/mission.json

@@ -0,0 +1,18 @@
+{
+  "operational_height": 1100,
+  "geofences": {
+    "polygons": [
+      {"points": [[12543.4213, 23476.324], [123.312, 984356.345]], "fence_type": "INCLUSION"},
+      {"points": [[12543.4213, 23476.324], [123.312, 984356.345]], "fence_type": "EXCLUSION"}
+    ]
+  },
+  "action_points": [
+    {"point": [-35.35967231, 149.16146047], "height": 500, "action_enum": "waypoint"},
+    {"point": [-35.35973951, 149.16801175], "height": 400, "action_enum": "search", "action_specific": {"targets": ["artillery"]}},
+    {"point": [-35.36408532, 149.16816283], "height": 300, "action_enum": "search", "action_specific": {"targets": ["artillery", "tank"]}},
+    {"point": [-35.36546294, 149.16479791], "height": 300, "action_enum": "search", "action_specific": {"targets": ["tank"]}},
+    {"point": [-35.36364851, 149.16073255], "height": 500, "action_enum": "return"}
+  ],
+  "return_point": [-35.36286449, 149.16534729]
+}
+