Initial version of ArduPilot compatible autopilot

- removed PX4 and MAVSDK git submodules

src/az_mission_controller.cpp

@@ -0,0 +1,92 @@
+#include <QDebug>
+#include <mavsdk/plugins/telemetry/telemetry.h>
+#include "az_mission_controller.h"
+#include "az_mission.h"
+#include "az_utils.h"
+
+
+AzMissionController::AzMissionController(AzMission &mission, QObject *parent) :
+    QObject(parent),
+    mMission(mission),
+    mAction(nullptr),
+    mMissionStarted(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()) {
+        qDebug() << "AzMissionController::flyToNextMissionItem() All mission items are handled. Stopping mission handling.";
+        mMissionStarted = false;
+        emit finished();
+        return true;
+    }
+
+    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;
+
+    qDebug() << "AzMissionController::flyToNextMissionItem() MAVSDK::Action::goto_location() starts";
+    Action::Result result = mAction->goto_location(coordinate.latitude, coordinate.longitude, mAbsoluteAltitude, 0.0f);
+    qDebug() << "AzMissionController::flyToNextMissionItem() MAVSDK::Action::goto_location() ends";
+
+    if (result == Action::Result::Success) {
+        emit missionProgressChanged(mCurrentMissionIndex + 1, mMission.getActionPoints().size());
+        return true;
+    }
+
+    return false;
+}
+
+
+void AzMissionController::newPosition(Telemetry::Position position)
+{
+    (void)position;
+
+    if (mMissionStarted == false) {
+        return;
+    }
+
+    const AzCoordinate &coordinate = mMission.getActionPoints().at(mCurrentMissionIndex).getPoint();
+    float distanceToTarget = AzUtils::distance(position.latitude_deg,
+                                               position.longitude_deg,
+                                               coordinate.latitude,
+                                               coordinate.longitude);
+
+    qDebug() << "AzMissionController::newPosition() distanceToTarget: " << distanceToTarget;
+
+    // TODO!! In final application we need to
+    if (distanceToTarget <= 0.01) {
+        qDebug() << "AzMissionController::newPosition() target reached. Continuing to the next item.";
+        flyToNextMissionItem();
+    }
+}