Update README.md

Added link to Install MAVSDK for Ubuntu 24.04 (libmavsdk-dev3.0.0)

README.md

@@ -32,6 +32,13 @@ wget https://github.com/mavlink/MAVSDK/releases/download/v2.12.10/libmavsdk-dev_
 sudo dpkg -i libmavsdk-dev_2.12.10_ubuntu22.04_amd64.deb
 ```
 
+
+## Install MAVSDK for Ubuntu 24.04 (libmavsdk-dev3.0.0)
+```bash
+wget https://github.com/mavlink/MAVSDK/releases/download/v3.0.0/libmavsdk-dev_3.0.0_ubuntu24.04_amd64.deb
+sudo dpkg -i libmavsdk-dev_3.0.0_ubuntu24.04_amd64.deb
+```
+
 ## Install ONNX Runtime for Ubuntu (not required for embedded platforms)
 
 ### With GPU inference

ai_controller/ai_controller.pro

@@ -1,4 +1,4 @@
-QT += core network serialport
+QT += core network
 QT -= gui
 CONFIG += concurrent console c++17
 MOC_DIR = moc

ai_controller/aiengine.cpp

@@ -3,7 +3,9 @@
 
 #include "aiengine.h"
 #include "aiengineinference.h"
+#ifdef SAVE_IMAGES
 #include "aiengineimagesaver.h"
+#endif
 
 #if defined(OPI5_BUILD)
 #include "src-opi5/aiengineinferenceopi5.h"
@@ -17,8 +19,11 @@
 
 
 
-AiEngine::AiEngine(QString modelPath, QObject *parent)
-    : QObject{parent}
+AiEngine::AiEngine(QString modelPath, QObject *parent) :
+    QObject{parent},
+    mRtspFrameCounter(0),
+    mInferenceFrameCounter(0)
+
 {
     mRtspListener = new AiEngineRtspListener(this);
     connect(mRtspListener, &AiEngineRtspListener::frameReceived, this, &AiEngine::frameReceivedSlot);
@@ -69,7 +74,7 @@ AiEngine::AiEngine(QString modelPath, QObject *parent)
 void AiEngine::start(void)
 {
     mRtspListener->startListening();
-    mElapsedTimer.start();
+    mRtspElapsedTimer.start();
 }
 
 
@@ -81,16 +86,15 @@ void AiEngine::stop(void)
 
 void AiEngine::inferenceResultsReceivedSlot(AiEngineInferenceResult result)
 {
-    mFrameCounter++;
-    qDebug() << "FPS = " << (mFrameCounter / (mElapsedTimer.elapsed()/1000.0f));
-    //qDebug() << "DEBUG. inference frame counter:" << mFrameCounter;
+    mInferenceFrameCounter++;
+    float fps =mRtspElapsedTimer.elapsed() == 0 ? 0 : (mInferenceFrameCounter / (mRtspElapsedTimer.elapsed()/1000.0f));
+    printf("Analyzed %d/%d frames with AI. FPS=%.1f\n", mInferenceFrameCounter, mRtspFrameCounter, fps);
 
-    //qDebug() << "AiEngine got inference results in thread: " << QThread::currentThreadId();
     if (mGimbalClient != nullptr) {
         mGimbalClient->inferenceResultSlot(result);
     }
 
-    cv::imshow("Received Frame", result.frame);
+    cv::imshow("AI Engine", result.frame);
 
 #ifdef SAVE_IMAGES
     static int imageCounter = 0;
@@ -103,26 +107,17 @@ void AiEngine::inferenceResultsReceivedSlot(AiEngineInferenceResult result)
 
 void AiEngine::frameReceivedSlot(cv::Mat frame)
 {
-    //qDebug() << "AiEngine got frame from RTSP listener in thread: " << QThread::currentThreadId();
-    //cv::imshow("Received Frame", frame);
-    static int framecounter = 0;
-    //qDebug() << "DEBUG. RTSP frame counter:" << framecounter;
+    mRtspFrameCounter++;
 
     if (mInference->isActive() == false) {
-        //qDebug() << "AiEngine. Inference thread is free. Sending frame to it.";
         emit inferenceFrame(frame);
-        framecounter++;
     }
 #ifdef OPI5_BUILD
     else if (mInference2->isActive() == false) {
-        //qDebug() << "AiEngine. Inference thread is free. Sending frame to it.";
         emit inferenceFrame2(frame);
-        framecounter++;
     }
     else if (mInference3->isActive() == false) {
-        //qDebug() << "AiEngine. Inference thread is free. Sending frame to it.";
         emit inferenceFrame3(frame);
-        framecounter++;
     }
 #endif
 }

ai_controller/aiengine.h

@@ -1,7 +1,7 @@
 #pragma once
 
-#include <QObject>
 #include <QElapsedTimer>
+#include <QObject>
 #include <opencv2/core.hpp>
 #include <opencv2/videoio.hpp>
 #include "aienginertsplistener.h"
@@ -26,8 +26,9 @@ signals:
     void inferenceFrame3(cv::Mat frame);
 
 private:
-    QElapsedTimer mElapsedTimer;
-    uint32_t mFrameCounter = 0;
+    uint32_t mRtspFrameCounter;
+    uint32_t mInferenceFrameCounter;
+    QElapsedTimer mRtspElapsedTimer;
     AiEngineRtspListener *mRtspListener;
     AiEngineInference *mInference;
     AiEngineInference *mInference2;

ai_controller/aienginegimbalserver.cpp

@@ -11,10 +11,12 @@ AiEngineGimbalServer::AiEngineGimbalServer(QObject *parent)
     mIsAvailable = false;
     qDebug() << "Initial is available: " << mIsAvailable;
 
-    QTimer::singleShot(5000, this, [this]() {
-        mIsAvailable = true;
-        qDebug() << "Initial is available: " << mIsAvailable;
-    });
+    // Commented out, might be required later
+    // Making camera available is currently do in processUdpCommands() with command bringCameraDown
+    //QTimer::singleShot(5000, this, [this]() {
+    //    mIsAvailable = true;
+    //    qDebug() << "Initial is available: " << mIsAvailable;
+    //});
 
     mActions.setup(&mUdpSocket, &mUdpCommand, &mUdpResponse, &mGimbalStatus);
 
@@ -26,6 +28,13 @@ AiEngineGimbalServer::AiEngineGimbalServer(QObject *parent)
     mDronePosition.yaw = 90.0;
 
     connect(&mActions, &AiEngineGimbalServerActions::aiTargetZoomed, this, &AiEngineGimbalServer::aiTargetZoomed);
+
+    // Create and bind the new UDP socket for receiving commands
+    mReceiveUdpSocket = new QUdpSocket(this);
+    mReceiveUdpSocket->bind(QHostAddress::LocalHost, 45454);
+    
+    // Connect the socket to handle incoming messages
+    connect(mReceiveUdpSocket, &QUdpSocket::readyRead, this, &AiEngineGimbalServer::processUdpCommands);
 }
 
 
@@ -83,7 +92,10 @@ void AiEngineGimbalServer::zoomToAiTargetSlot(AiEngineCameraTarget target)
         // Allow calls
         int delay4 = delay3 + 3000; // Adjust this value as needed
         QTimer::singleShot(delay4, this, [this]() {
+            // Only make camera available for commands if it is allowed
+            if (mActions.getAllowCameraCommands() == true) {
                 mIsAvailable = true;
+            }
             qDebug() << "Is available: " << mIsAvailable;
         });
     }
@@ -101,3 +113,51 @@ void AiEngineGimbalServer::cameraPositionSlot(AiEngineCameraPosition position)
 {
     qDebug() << "AiEngineGimbalServer::cameraPositionSlot() Move camera to:" << position.pitch <<  position.yaw << "zoom:" << position.zoom;
 }
+
+
+void AiEngineGimbalServer::processUdpCommands(void) {
+    while (mReceiveUdpSocket->hasPendingDatagrams()) {
+        QByteArray datagram;
+        datagram.resize(mReceiveUdpSocket->pendingDatagramSize());
+        mReceiveUdpSocket->readDatagram(datagram.data(), datagram.size());
+
+        QString command = QString::fromUtf8(datagram);
+        qDebug() << "Received command:" << command;
+
+        if (command == "bringCameraDown") {
+            mActions.setAllowCameraCommands(true); // This will allow other camera commands
+            mActions.goToInitialOrientation();
+
+            // 10 second delay to let camera get ready for AI
+            QTimer::singleShot(10000, this, [this]() {
+                mIsAvailable = true;
+                qDebug() << "Camera set available and to initial position";
+            });
+        }
+        else if (command == "bringCameraUp") {
+            // No delay, set camera unavailable
+            mActions.setAllowCameraCommands(false); // This will prevent any ongoing commands
+            mIsAvailable = false;
+            mActions.goToInitialOrientation();
+            qDebug() << "Camera set unavailable and to initial position";
+        }
+    }
+
+    /*
+    // How to call this:
+
+    #include <QUdpSocket>
+
+    void sendCommand(const QString &command) {
+        QUdpSocket udpSocket;
+        QByteArray datagram = command.toUtf8();
+        udpSocket.writeDatagram(datagram, QHostAddress::LocalHost, 45454);
+    }
+
+    int main() {
+        sendCommand("bringCameraDown");
+        sendCommand("bringCameraUp");
+        return 0;
+    }
+    */
+}

ai_controller/aienginegimbalserver.h

@@ -2,6 +2,7 @@
 
 #include <QObject>
 #include <QMap>
+#include <QUdpSocket>
 #include "aienginedefinitions.h"
 #include "aienginegimbalserveractions.h"
 #include "aienginegimbalserverudpcommand.h"
@@ -34,4 +35,8 @@ private:
     AiEngineGimbalServerUDPResponse mUdpResponse;
     AiEngineGimbalServerActions mActions;
     bool mIsAvailable;
+    QUdpSocket mReceiveUdpSocket; // UDP socket for receiving commands
+
+private slots:
+    void processPendingDatagrams(void); // Handles incoming UDP messages
 };

ai_controller/aienginegimbalserveractions.cpp

@@ -17,6 +17,7 @@ void AiEngineGimbalServerActions::setup(AiEngineGimbalServerUDP *udpSocket, AiEn
     mUdpCommand = udpCommand;
     mUdpResponse = udpResponse;
     mGimbalStatus = gimbalStatus;
+    mAllowCameraCommands = false;
 
     // Set initial position and update status
     QByteArray tempCommand;
@@ -68,24 +69,8 @@ void AiEngineGimbalServerActions::setup(AiEngineGimbalServerUDP *udpSocket, AiEn
     float maxZoom = responseValues["zoom"].toFloat();
     mGimbalStatus->maxZoom = maxZoom > 10 ? 10 : maxZoom;
 
-    // Go to initial orientation
-    tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::TURN_TO_DEGREES);
-    int16_t degreesVal = AI_ENGINE_GIMBAL_INITIAL_YAW * 10;
-    tempCommand[8] = degreesVal & 0xFF;
-    tempCommand[9] = degreesVal >> 8;
-    degreesVal = AI_ENGINE_GIMBAL_INITIAL_PITCH * 10;
-    tempCommand[10] = degreesVal & 0xFF;
-    tempCommand[11] = degreesVal >> 8;
-    mUdpSocket->sendCommand(tempCommand);
-
-    // Go to initial zoom
-    tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::ZOOM_TO_X);
-    uint8_t integerPart = static_cast<uint8_t>(AI_ENGINE_CAMERA_INITIAL_ZOOM);
-    float fractionalPart = AI_ENGINE_CAMERA_INITIAL_ZOOM - integerPart;
-    uint8_t scaledFractional = uint8_t(fractionalPart * 10);
-    tempCommand[8] = integerPart;
-    tempCommand[9] = scaledFractional;
-    mUdpSocket->sendCommand(tempCommand);
+    // Go to initial orientation and zoom
+    goToInitialOrientation();
 
     mGimbalStatus->currentPitch = AI_ENGINE_GIMBAL_INITIAL_PITCH;
     mGimbalStatus->currentRoll = AI_ENGINE_GIMBAL_INITIAL_ROLL;
@@ -178,6 +163,7 @@ void AiEngineGimbalServerActions::getAnglesToOnScreenTarget(int targetX, int tar
 
 void AiEngineGimbalServerActions::turnToTarget(AiEngineRectangleProperties rectangle)
 {
+    if (mAllowCameraCommands == true) {
         qDebug().noquote().nospace() << "Turning to target";
 
         float resultYaw = 0.0f;
@@ -196,10 +182,12 @@ void AiEngineGimbalServerActions::turnToTarget(AiEngineRectangleProperties recta
 
         mUdpSocket->sendCommand(serialCommandTurn);
     }
+}
 
 
 void AiEngineGimbalServerActions::zoomToTarget(AiEngineRectangleProperties rectangle)
 {
+    if (mAllowCameraCommands == true) {
         qDebug().noquote().nospace() << "Zooming to target";
 
         float fillRatio = 0.5;
@@ -225,6 +213,7 @@ void AiEngineGimbalServerActions::zoomToTarget(AiEngineRectangleProperties recta
 
         mUdpSocket->sendCommand(serialCommandNewZoom);
     }
+}
 
 
 void AiEngineGimbalServerActions::getLocation(AiEngineDronePosition dronePosition, int targetIndex)
@@ -251,6 +240,7 @@ void AiEngineGimbalServerActions::getLocation(AiEngineDronePosition dronePositio
 
 void AiEngineGimbalServerActions::restoreOrientationAndZoom(AiEngineGimbalStatus gimbalStatus)
 {
+    if (mAllowCameraCommands == true) {
         QByteArray tempCommand;
 
         // Go to initial orientation
@@ -271,6 +261,7 @@ void AiEngineGimbalServerActions::restoreOrientationAndZoom(AiEngineGimbalStatus
         tempCommand[8] = integerPart;
         tempCommand[9] = scaledFractional;
         mUdpSocket->sendCommand(tempCommand);
+    }
 
     // TODO: Maybe send signal that all done?
 }
@@ -354,3 +345,40 @@ float AiEngineGimbalServerActions::degreesToRadians(float degrees)
 {
     return degrees * M_PI / 180.0f;
 }
+
+
+void AiEngineGimbalServerActions::goToInitialOrientation(void)
+{
+    // These camera commands should always be allowed
+
+    // Go to initial orientation
+    tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::TURN_TO_DEGREES);
+    int16_t degreesVal = AI_ENGINE_GIMBAL_INITIAL_YAW * 10;
+    tempCommand[8] = degreesVal & 0xFF;
+    tempCommand[9] = degreesVal >> 8;
+    degreesVal = AI_ENGINE_GIMBAL_INITIAL_PITCH * 10;
+    tempCommand[10] = degreesVal & 0xFF;
+    tempCommand[11] = degreesVal >> 8;
+    mUdpSocket->sendCommand(tempCommand);
+
+    // Go to initial zoom
+    tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::ZOOM_TO_X);
+    uint8_t integerPart = static_cast<uint8_t>(AI_ENGINE_CAMERA_INITIAL_ZOOM);
+    float fractionalPart = AI_ENGINE_CAMERA_INITIAL_ZOOM - integerPart;
+    uint8_t scaledFractional = uint8_t(fractionalPart * 10);
+    tempCommand[8] = integerPart;
+    tempCommand[9] = scaledFractional;
+    mUdpSocket->sendCommand(tempCommand);
+}
+
+
+bool AiEngineGimbalServerActions::getAllowCameraCommands(void)
+{
+    return mAllowCameraCommands;
+}
+
+
+void AiEngineGimbalServerActions::setAllowCameraCommands(bool allow)
+{
+    mAllowCameraCommands = allow;
+}

ai_controller/aienginegimbalserveractions.h

@@ -49,6 +49,9 @@ public slots:
     void zoomToTarget(AiEngineRectangleProperties rectangle);
     void getLocation(AiEngineDronePosition dronePosition, int targetIndex);
     void restoreOrientationAndZoom(AiEngineGimbalStatus gimbalStatus);
+    void goToInitialOrientation(void);
+    bool getAllowCameraCommands(void);
+    void setAllowCameraCommands(bool allow);
 
 signals:
     void aiTargetZoomed(AiEngineTargetPosition);
@@ -58,6 +61,7 @@ private:
     AiEngineGimbalServerUDPCommand *mUdpCommand;
     AiEngineGimbalServerUDPResponse *mUdpResponse;
     AiEngineGimbalStatus *mGimbalStatus;
+    bool mAllowCameraCommands;
 
 private slots:
     CameraData getCameraData(void);

ai_controller/aienginertsplistener.cpp

@@ -82,7 +82,11 @@ void AiEngineRtspListener::listenLoop(void)
 #else
     qDebug() << "AiEngineRtspListener loop running in thread: " << QThread::currentThreadId();
 
-    mCap.open(rtspVideoUrl.toStdString());
+    while (mCap.open(rtspVideoUrl.toStdString()) == false) {
+        qDebug() << "AiEngineRtspListener can't open video stream:" << rtspVideoUrl;
+        QThread::msleep(1000);
+    }
+
     cv::Mat frame;
 
     while (mIsListening) {

ai_controller/src-ncnn/aiengineinferencencnn.cpp

@@ -1,6 +1,5 @@
 #include <QDebug>
 #include <QThread>
-#include <iostream>
 #include <vector>
 #include "aiengineinferencencnn.h"
 
@@ -23,7 +22,7 @@ char* getCharPointerCopy(const QString& modelPath) {
 AiEngineInferencevNcnn::AiEngineInferencevNcnn(QString modelPath, QObject *parent) :
     AiEngineInference{modelPath, parent}
 {
-    qDebug() << "TUOMAS AiEngineInferencevNcnn() mModelPath=" << mModelPath;
+    qDebug() << "AiEngineInferencevNcnn() mModelPath=" << mModelPath;
 
     yolov8.opt.num_threads        = 4;
     yolov8.opt.use_vulkan_compute = false;
@@ -32,9 +31,6 @@ AiEngineInferencevNcnn::AiEngineInferencevNcnn(QString modelPath, QObject *paren
     char *model = getCharPointerCopy(modelPath);
     char *param = getCharPointerCopy(paramPath);
 
-    qDebug() << "model:" << model;
-    qDebug() << "param:" << param;
-
     yolov8.load_param(param);
     yolov8.load_model(model);
 }
@@ -229,8 +225,6 @@ int AiEngineInferencevNcnn::detect_yolov8(const cv::Mat& bgr, std::vector<Object
     const float norm_vals[3] = {1 / 255.f, 1 / 255.f, 1 / 255.f};
     in_pad.substract_mean_normalize(0, norm_vals);
 
-    auto start = std::chrono::high_resolution_clock::now();
-
     ncnn::Extractor ex = yolov8.create_extractor();
 
     ex.input("in0", in_pad);
@@ -283,9 +277,6 @@ int AiEngineInferencevNcnn::detect_yolov8(const cv::Mat& bgr, std::vector<Object
         objects[i].rect.height = y1 - y0;
     }
 
-    auto end = std::chrono::high_resolution_clock::now();
-    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
-    std::cout << "Time taken: " << duration.count() << " milliseconds" << std::endl;
     return 0;
 }
 
@@ -340,8 +331,7 @@ static cv::Mat draw_objects(const cv::Mat& bgr, const std::vector<Object>& objec
 
         cv::Scalar cc(color[0], color[1], color[2]);
 
-        fprintf(stderr, "%d = %.5f at %.2f %.2f %.2f x %.2f\n", obj.label, obj.prob,
-                obj.rect.x, obj.rect.y, obj.rect.width, obj.rect.height);
+        //fprintf(stderr, "%d = %.5f at %.2f %.2f %.2f x %.2f\n", obj.label, obj.prob, obj.rect.x, obj.rect.y, obj.rect.width, obj.rect.height);
 
         cv::rectangle(image, obj.rect, cc, 2);
 
@@ -378,7 +368,6 @@ void AiEngineInferencevNcnn::performInferenceSlot(cv::Mat frame)
     std::vector<Object> objects;
     detect_yolov8(scaledImage, objects);
 
-    if (objects.empty() == false) {
     AiEngineInferenceResult result;
     result.frame = draw_objects(scaledImage, objects);
 
@@ -396,8 +385,6 @@ void AiEngineInferencevNcnn::performInferenceSlot(cv::Mat frame)
     }
 
     emit resultsReady(result);
-    }
-
     mActive = false;
 }
 

ai_controller/src-onnx-runtime/aiengineinferenceonnxruntime.cpp

@@ -13,7 +13,7 @@ AiEngineInferencevOnnxRuntime::AiEngineInferencevOnnxRuntime(QString modelPath,
     AiEngineInference{modelPath, parent},
     mPredictor(modelPath.toStdString(), confThreshold, iouThreshold, maskThreshold)
 {
-    qDebug() << "TUOMAS AiEngineInferencevOnnxRuntime() mModelPath=" << mModelPath;
+    qDebug() << "AiEngineInferencevOnnxRuntime() mModelPath=" << mModelPath;
     qDebug() << "AiEngineInferencevOnnxRuntime() mClassNames.size() =" << mClassNames.size();
 }
 
@@ -49,7 +49,6 @@ cv::Mat AiEngineInferencevOnnxRuntime::drawLabels(const cv::Mat &image, const st
             cv::Scalar(0, 0, 0),
             1,
             cv::LINE_AA);
-
     }
 
     return result;
@@ -58,8 +57,6 @@ cv::Mat AiEngineInferencevOnnxRuntime::drawLabels(const cv::Mat &image, const st
 
 void AiEngineInferencevOnnxRuntime::performInferenceSlot(cv::Mat frame)
 {
-    qDebug() << __PRETTY_FUNCTION__;
-
     try {
         mActive = true;
         cv::Mat scaledImage                  = resizeAndPad(frame);
@@ -106,11 +103,8 @@ void AiEngineInferencevOnnxRuntime::performInferenceSlot(cv::Mat frame)
             result.objects.append(object);
         }
 
-        if (result.objects.empty() == false) {
         result.frame = drawLabels(scaledImage, detections);
         emit resultsReady(result);
-        }
-
         mActive = false;
     }
     catch (const cv::Exception& e) {

drone_controller/az_drone_controller.cpp

@@ -5,33 +5,27 @@
 #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)
+
+AzDroneController::AzDroneController(AzMission &azMission, QObject *parent)
     : QObject(parent)
-    , mMavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}
-    , // TODO!! Autopilot or CompanionComputer?
-    mDroneState(AZ_DRONE_STATE_DISCONNECTED)
+    , mMavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}} // TODO!! Autopilot or CompanionComputer?
+    , mDroneState(AZ_DRONE_STATE_DISCONNECTED)
+    , mMissionController(nullptr)
+    , mAzMission(azMission)
 {
     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";
 }
 
 
@@ -105,11 +99,13 @@ bool AzDroneController::stateGetTelemetryModule(void)
         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.
+    // 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;
 }
@@ -119,12 +115,7 @@ bool AzDroneController::stateGetActionModule(void)
 {
     mAction = new Action(mSystem);
 
-    if (mAction != nullptr) {
-        mMissionController->setAction(mAction);
-        return true;
-    }
-
-    return false;
+    return mAction == nullptr ? false : true;
 }
 
 
@@ -140,6 +131,16 @@ bool AzDroneController::stateHealthOk(void)
 }
 
 
+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();
@@ -156,11 +157,6 @@ bool AzDroneController::stateArm(void)
 
 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;
@@ -170,14 +166,20 @@ bool AzDroneController::stateTakeoff(void)
 
 bool AzDroneController::stateFlyMission(void)
 {
-    if (mCurrentPosition.relative_altitude_m < AZ_RELATIVE_FLY_ALTITUDE * 0.90) {
+    // 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);
+    //float flight_altitude_abs = AZ_RELATIVE_FLY_ALTITUDE + mFirstPosition.absolute_altitude_m;
+    //return mMissionController->startMissions(flight_altitude_abs);
+
+    return mMissionController->startMission();
 }
 
 
@@ -244,7 +246,19 @@ void AzDroneController::droneStateMachineSlot(void)
 
 void AzDroneController::newPositionSlot(Telemetry::Position position)
 {
-    cout << "[CONTROLLER] GPS position " << position.latitude_deg << ", " << position.longitude_deg << endl;
+    /*
+    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.
@@ -256,12 +270,19 @@ void AzDroneController::newPositionSlot(Telemetry::Position position)
     mCurrentPosition = position;
 
     // Send new position to the mission controller.
-    if (mDroneState == AZ_DRONE_STATE_FLY_MISSION) {
+    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;

drone_controller/az_drone_controller.h

@@ -5,6 +5,7 @@
 
 #include <mavsdk/mavsdk.h>
 #include <mavsdk/plugins/action/action.h>
+#include <mavsdk/plugins/mission_raw/mission_raw.h>
 #include <mavsdk/plugins/telemetry/telemetry.h>
 
 #include "az_mission.h"
@@ -19,6 +20,7 @@ typedef enum {
     AZ_DRONE_STATE_GOT_TELEMETRY_MODULE,
     AZ_DRONE_STATE_GOT_ACTION_MODULE,
     AZ_DRONE_STATE_HEALTH_OK,
+    AZ_DRONE_STATE_MISSION_UPLOADED,
     AZ_DRONE_STATE_ARMED,
     AZ_DRONE_STATE_TAKE_OFF,
     AZ_DRONE_STATE_AUTO_MODE_ACTIVATED,
@@ -44,8 +46,18 @@ protected:
     bool stateGetActionModule(void);
     bool stateHealthOk(void);
     bool stateArm(void);
+    bool stateUploadMission(void);
     bool stateTakeoff(void);
     bool stateFlyMission(void);
+    void setRawMission(void);
+    MissionRaw::MissionItem makeRawMissionItem(float latitude_deg1e7,
+                                               float longitude_deg1e7,
+                                               int32_t altitude_m,
+                                               float do_photo,
+                                               MAV_FRAME frame,
+                                               MAV_CMD command,
+                                               float p2 = 0,
+                                               float p3 = 0);
 
     // Slots that are called by the emitted Qt signals.
 protected slots:
@@ -59,6 +71,9 @@ protected slots:
     // Slot that is called when the newPosition() signal below is emitted.
     void newPositionSlot(Telemetry::Position);
 
+    // Slot that is called when the newHeading() signal below is emitted.
+    void newHeadingSlot(double heading);
+
     // 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);
@@ -75,6 +90,11 @@ signals:
     // captured in the main thread to avoid threading issues.
     void newPosition(Telemetry::Position);
 
+    // Signal is emitted when Ardupilot sends a new heading from the
+    // MAVSDK thread. Signal goes through the main event loop and is
+    // captured in the main thread to avoid threading issues.
+    void newHeading(double heading);
+
     // 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);
@@ -88,4 +108,7 @@ protected:
     Telemetry::Position mFirstPosition;
     Telemetry::Position mCurrentPosition;
     AzMissionController *mMissionController;
+    bool isCopterType;
+    int mMissionItemSeqNum;
+    AzMission &mAzMission;
 };

drone_controller/az_drone_controller_plane.cpp

@@ -34,7 +34,8 @@ void AzDroneControllerPlane::droneStateMachineSlot(void)
            {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_HEALTH_OK, &AzDroneControllerPlane::stateUploadMission, "Uploading the mission", 1000},
+           {AZ_DRONE_STATE_MISSION_UPLOADED, &AzDroneControllerPlane::stateWaitAutoSwitch, "Waiting user to switch to ArduPilot AUTO mode", 1000},
            {AZ_DRONE_STATE_AUTO_MODE_ACTIVATED, &AzDroneControllerPlane::stateFlyMission, "Starting the mission", 1000},
            {AZ_DRONE_STATE_FLY_MISSION, nullptr, "Mission started", 0}};
 

drone_controller/az_mission_controller.cpp

@@ -1,121 +1,166 @@
 #include <QCoreApplication>
 #include <QDebug>
 
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/action/action.h>
+#include <mavsdk/plugins/mission/mission.h>
 #include <mavsdk/plugins/telemetry/telemetry.h>
+#include <mavsdk/plugins/mission_raw/mission_raw.h>
+#include <mavsdk/plugins/offboard/offboard.h>
 
 #include "az_mission.h"
 #include "az_mission_controller.h"
 #include "az_utils.h"
 
-AzMissionController::AzMissionController(AzMission &mission, QObject *parent)
+
+AzMissionController::AzMissionController(System &system, AzMission &mission, QObject *parent)
     : QObject(parent)
-    , mMission(mission)
-    , mAction(nullptr)
+    , mAzMission(mission)
     , mMissionStarted(false)
-    , mFlyingToReturnPoint(false)
-    , mAbsoluteAltitude(-10000)
+    , mMissionItemSeqNum(0)
+    , mMissionRaw(nullptr)
+    , mSystem(system)
 {}
 
-void AzMissionController::setAction(Action *action)
+
+MissionRaw::MissionItem AzMissionController::makeRawMissionItem(
+    float latitudeDeg1e7,
+    float longitudeDeg1e7,
+    int32_t altitude,
+    float doPhoto,
+    MAV_FRAME frame,
+    MAV_CMD command,
+    float p2,
+    float p3)
 {
-    mAction = action;
+    (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       = doPhoto;
+    new_item.x            = latitudeDeg1e7 * 1e7;
+    new_item.y            = longitudeDeg1e7 * 1e7;
+    new_item.z            = altitude;
+    new_item.mission_type = MAV_MISSION_TYPE_MISSION;
+    new_item.autocontinue = 1;
+
+    if (mMissionItemSeqNum == 1) {
+        new_item.current = 1;
     }
 
-bool AzMissionController::startMissions(float absoluteAltitude)
-{
-    mCurrentMissionIndex = -1;
-    mMissionStarted      = true;
-    mAbsoluteAltitude    = absoluteAltitude; // TODO!! Use altitudes from the JSON file.
-    return flyToNextMissionItem();
+    mMissionItemSeqNum++;
+
+    return new_item;
 }
 
-void AzMissionController::stopMissions(void)
+
+bool AzMissionController::uploadMission(void)
 {
-    // TODO!! Needs to be improved. At least send signal to AzDroneController.
-    mMissionStarted = false;
+    cout << "[MISSION] stateUploadMission() Setting raw mission starts" << endl;
+
+    mMissionItemSeqNum = 0;
+
+    if (mMissionRaw == nullptr) {
+        mMissionRaw = new MissionRaw(mSystem);
     }
 
-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;
+    auto clearResult = mMissionRaw->clear_mission();
+    if (clearResult != MissionRaw::Result::Success) {
+        std::cout << "[MISSION] stateUploadMission() Clearing mMissionRaw failed" << std::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;
+    auto downloadResult = mMissionRaw->download_mission();
+    if (downloadResult.first != MissionRaw::Result::Success) {
+        std::cout << "[MISSION] 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;
+    cout << "[MISSION] Home location: " << latDeg << ", " << lonDeg << endl;
+
+    // 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));
+
+    // Action points
+    const vector<AzActionPoint> &azMissions = mAzMission.getActionPoints();
+    for (uint i = 0; i < azMissions.size(); i++) {
+        missionPlan.push_back(makeRawMissionItem(
+            azMissions[i].getPoint().latitude,
+            azMissions[i].getPoint().longitude,
+            azMissions[i].getHeight(),
+            0,
+            MAV_FRAME_GLOBAL_TERRAIN_ALT,
+            MAV_CMD_NAV_WAYPOINT));
     }
 
+    // Landing point
+    missionPlan.push_back(makeRawMissionItem(
+        mAzMission.getReturnPoint().latitude,
+        mAzMission.getReturnPoint().longitude,
+        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 << "[MISSION] stateUploadMission() seq: " << (int)item.seq << '\n';
+    }
+
+    auto uploadResult = mMissionRaw->upload_mission(missionPlan);
+    if (uploadResult != MissionRaw::Result::Success) {
+        std::cout << "[MISSION] stateUploadMission() upload failed. Result: " << uploadResult << std::endl;
+        return false;
+    }
+
+    mMissionRaw->set_current_mission_item(0);
+    cout << "[MISSION] stateUploadMission() Setting raw mission ends" << endl;
+
+    return true;
+}
+
+
+bool AzMissionController::startMission()
+{
+    MissionRaw::Result startResult = mMissionRaw->start_mission();
+    cout << "[MISSION] Start mission result:" << startResult << endl;
+    return startResult == MissionRaw::Result::Success;
+}
+
+
+void AzMissionController::stopMission(void)
+{
+    mMissionRaw->pause_mission();
+}
+
+
 void AzMissionController::newPosition(Telemetry::Position pos)
 {
+    (void)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

@@ -1,12 +1,12 @@
 #pragma once
 
 #include <QObject>
-#include <QElapsedTimer>
 
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/mission_raw/mission_raw.h>
 #include <mavsdk/plugins/telemetry/telemetry.h>
 
 #include "az_mission.h"
-#include "plugins/action/action.h"
 
 using namespace mavsdk;
 
@@ -14,10 +14,20 @@ 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);
+    explicit AzMissionController(System &system, AzMission &mission, QObject *parent = nullptr);
+    bool startMission();
+    void stopMission(void);
+    bool uploadMission(void);
+
+private:
+    MissionRaw::MissionItem makeRawMissionItem(float latitudeDeg1e7,
+                                               float longitudeDeg1e7,
+                                               int32_t altitude,
+                                               float doPhoto,
+                                               MAV_FRAME frame,
+                                               MAV_CMD command,
+                                               float p2 = 0,
+                                               float p3 = 0);
 
 public slots:
     void newPosition(Telemetry::Position position);
@@ -27,13 +37,10 @@ signals:
     void finished(void);
 
 private:
-    bool flyToNextMissionItem(void);
-
-    AzMission &mMission;
-    Action *mAction;
+    AzMission &mAzMission;
     int mCurrentMissionIndex;
     bool mMissionStarted;
-    bool mFlyingToReturnPoint;
-    float mAbsoluteAltitude;
-    QElapsedTimer mPlaneCirclingTime;
+    int mMissionItemSeqNum;
+    MissionRaw *mMissionRaw;
+    System &mSystem;
 };

drone_controller/main.cpp

@@ -25,7 +25,7 @@ int main(int argc, char *argv[])
         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";
+        qCritical() << "\tFor example ./drone_controller mission.json plane udp";
         return 1;
     }
 

drone_controller/mission.json

@@ -7,11 +7,11 @@
     ]
   },
   "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"}
+    {"point": [-35.35967231, 149.16146047], "height": 50, "action_enum": "waypoint"},
+    {"point": [-35.35973951, 149.16801175], "height": 50, "action_enum": "search", "action_specific": {"targets": ["artillery"]}},
+    {"point": [-35.36408532, 149.16816283], "height": 50, "action_enum": "search", "action_specific": {"targets": ["artillery", "tank"]}},
+    {"point": [-35.36546294, 149.16479791], "height": 50, "action_enum": "search", "action_specific": {"targets": ["tank"]}},
+    {"point": [-35.36364851, 149.16073255], "height": 50, "action_enum": "return"}
   ],
   "return_point": [-35.36286449, 149.16534729]
 }

misc/map_tool/find_nearest_tiles/find_nearest_tiles.pro

@@ -0,0 +1,15 @@
+QT = core
+
+CONFIG += c++17 cmdline
+
+# You can make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+        main.cpp
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target

misc/map_tool/find_nearest_tiles/main.cpp

@@ -0,0 +1,99 @@
+#include <QCoreApplication>
+
+#include <QtCore>
+#include <cmath>
+#include <iostream>
+
+// Structure to store tile information
+struct Tile {
+    int row;
+    int column;
+    double topLeftLat;
+    double topLeftLon;
+    double bottomRightLat;
+    double bottomRightLon;
+    double centerLat;
+    double centerLon;
+
+    // Constructor
+    Tile(int r, int c, double tlLat, double tlLon, double brLat, double brLon)
+        : row(r), column(c), topLeftLat(tlLat), topLeftLon(tlLon),
+        bottomRightLat(brLat), bottomRightLon(brLon) {
+        centerLat = (topLeftLat + bottomRightLat) / 2.0;
+        centerLon = (topLeftLon + bottomRightLon) / 2.0;
+    }
+};
+
+// Function to calculate Haversine distance between two geographic points
+double haversine(double lat1, double lon1, double lat2, double lon2) {
+    constexpr double R = 6371.0; // Earth radius in kilometers
+    double dLat = qDegreesToRadians(lat2 - lat1);
+    double dLon = qDegreesToRadians(lon2 - lon1);
+    double a = std::sin(dLat / 2) * std::sin(dLat / 2) +
+               std::cos(qDegreesToRadians(lat1)) * std::cos(qDegreesToRadians(lat2)) *
+                   std::sin(dLon / 2) * std::sin(dLon / 2);
+    double c = 2 * std::atan2(std::sqrt(a), std::sqrt(1 - a));
+    return R * c;
+}
+
+// Function to parse filenames and extract Tile information
+QVector<Tile> parseTiles(const QStringList &filenames) {
+    QVector<Tile> tiles;
+    static const QRegularExpression regex(R"(map_(\d+)_(\d+)_tl_([-\d\.]+)_([-\d\.]+)_br_([-\d\.]+)_([-\d\.]+)\.)");
+
+    for (const QString &filename : filenames) {
+        QRegularExpressionMatch match = regex.match(filename);
+        if (match.hasMatch()) {
+            int row = match.captured(1).toInt();
+            int column = match.captured(2).toInt();
+            double topLeftLat = match.captured(3).toDouble();
+            double topLeftLon = match.captured(4).toDouble();
+            double bottomRightLat = match.captured(5).toDouble();
+            double bottomRightLon = match.captured(6).toDouble();
+
+            tiles.append(Tile(row, column, topLeftLat, topLeftLon, bottomRightLat, bottomRightLon));
+        }
+    }
+    return tiles;
+}
+
+// Main function to find the nearest tiles
+QVector<Tile> findNearestTiles(const QString &directoryPath, double currentLat, double currentLon) {
+    QDir directory(directoryPath);
+    QStringList filenames = directory.entryList(QStringList() << "*.jpg" << "*.png", QDir::Files);
+    QVector<Tile> tiles = parseTiles(filenames);
+
+    // Sort tiles based on Haversine distance
+    std::sort(tiles.begin(), tiles.end(), [currentLat, currentLon](const Tile &a, const Tile &b) {
+        return haversine(currentLat, currentLon, a.centerLat, a.centerLon) <
+               haversine(currentLat, currentLon, b.centerLat, b.centerLon);
+    });
+
+    return tiles;
+}
+
+int main(int argc, char *argv[]) {
+    QCoreApplication app(argc, argv);
+
+    if (argc != 4) {
+        std::cerr << "Usage: " << argv[0] << " <directory> <latitude> <longitude>" << std::endl;
+        return -1;
+    }
+
+    QString directoryPath = argv[1];
+    double currentLat = QString(argv[2]).toDouble();
+    double currentLon = QString(argv[3]).toDouble();
+
+    qInfo().noquote().nospace() << "Directory:  " << directoryPath;
+    qInfo().noquote().nospace() << "CurrentLat: " << currentLat;
+    qInfo().noquote().nospace() << "CurrentLon: " << currentLon;
+
+    QVector<Tile> sortedTiles = findNearestTiles(directoryPath, currentLat, currentLon);
+
+    for (const Tile &tile : sortedTiles) {
+        std::cout << "Row: " << tile.row << ", Column: " << tile.column
+                  << ", Center: (" << tile.centerLat << ", " << tile.centerLon << ")\n";
+    }
+
+    return 0;
+}