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/
2026-04-22 08:16:34 +00:00 · 2024-10-19 14:44:34 +02:00
parent 54b7dc41ca
commit 45c19baa45
94 changed files with 149 additions and 204 deletions
@@ -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();
+            }
+        }
+    }
+}