Merge branch 'master' of github.com:azaion/autopilot

src/autopilot.pro

@@ -1,9 +1,12 @@
 QT = core
-CONFIG += c++17 cmdline link_pkgconfig
+CONFIG += c++17 cmdline
-PKGCONFIG += mavsdk
 
 QMAKE_CXXFLAGS += -Wno-address-of-packed-member
 
+# MAVSDK pkg-config file is garbage. Add dependency manually
+INCLUDEPATH += /usr/include/mavsdk
+LIBS += /usr/lib/libmavsdk.so.2
+
 SOURCES += \
         az_action_point.cpp \
         az_coordinate.cpp \

src/az_config.h

@@ -2,5 +2,5 @@
 
 #define FUNCTION_NAME(func) func()
 
-const char *AZ_CONNECTION           = "udp://:14550";
+const char *AZ_CONNECTION          = "udp://:14550";
-const int AZ_RELATIVE_FLY_ALTITUDE  = 50;
+const int AZ_RELATIVE_FLY_ALTITUDE = 50;

src/az_drone_controller.cpp

@@ -1,26 +1,38 @@
-#include <mavsdk/plugins/action/action.h>
-#include <mavsdk/plugins/telemetry/telemetry.h>
 #include <QCoreApplication>
 #include <QDebug>
 #include <QThread>
 #include <QTimer>
-#include <unistd.h>
+
+#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) :
+AzDroneController::AzDroneController(AzMission &mission, QObject *parent)
-    QObject(parent),
+    : QObject(parent)
-    mMavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}, // TODO!! Autopilot or CompanionComputer?
+    , mMavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}
+    , // TODO!! Autopilot or CompanionComputer?
     mDroneState(AZ_DRONE_STATE_DISCONNECTED)
 {
     qDebug() << "AzDroneController::AzDroneController() Thread ID: " << QThread::currentThreadId();
     mFirstPosition.relative_altitude_m = -10000;
 
     mMissionController = new AzMissionController(mission, this);
-    connect(mMissionController, &AzMissionController::missionProgressChanged, this, &AzDroneController::missionIndexChangedSlot);
-    connect(mMissionController, &AzMissionController::finished, this, &AzDroneController::missionFinishedSlot);
-}
 
+    // Mission progress from the AzMissionController. Slot will be used to find the targets later.
+    connect(
+        mMissionController,
+        &AzMissionController::missionProgressChanged,
+        this,
+        &AzDroneController::missionIndexChangedSlot);
+
+    // Mission controller signal to the slot in this class to finish the flying.
+    connect(mMissionController, &AzMissionController::finished, this, &AzDroneController::missionFinishedSlot);
+
+    // Healt info update from MAVSDK.
+    connect(this, &AzDroneController::newHealthInfo, this, &AzDroneController::newHealthInfoSlot, Qt::QueuedConnection);
+}
 
 void AzDroneController::start()
 {
@@ -28,25 +40,26 @@ void AzDroneController::start()
     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);
 }
 
-
 bool AzDroneController::stateConnect(void)
 {
+    // Connects to the flight controller based on AZ_CONNECTION define in AzConfig.
+    // TODO!! Add command line option to change between UDP and UART connections.
     return mMavsdk.add_any_connection(AZ_CONNECTION) == ConnectionResult::Success;
 }
 
-
 bool AzDroneController::stateAutopilot(void)
 {
+    // Get the first autopilot from the flight controller.
     return (mSystem = mMavsdk.first_autopilot(3.0).value()) != nullptr;
 }
 
-
 bool AzDroneController::stateTelemetryModule(void)
 {
     if ((mTelemetry = new Telemetry(mSystem)) == nullptr) {
@@ -55,29 +68,26 @@ bool AzDroneController::stateTelemetryModule(void)
 
     const auto setRateResult = mTelemetry->set_rate_position(1); // 1 Hz
     if (setRateResult != Telemetry::Result::Success) {
-        qCritical() << "Setting rate failed: " << (int)setRateResult;
+        qCritical() << "Setting rate failed: " << (int) setRateResult;
         return false;
     }
 
-    // Subscripe to position updates. Updated comes from different MAVSDK thread and so
+    // Subscripe to position updates. Updated comes from different MAVSDK thread. Send position
-    // we send it as event to this function so that it's handled in the main thread.
+    // as signal to this class (Qt::QueuedConnection) so that it's handled in the main thread.
     connect(this, &AzDroneController::newPosition, this, &AzDroneController::newPositionSlot, Qt::QueuedConnection);
-    mTelemetry->subscribe_position([this](Telemetry::Position position) {
+    mTelemetry->subscribe_position([this](Telemetry::Position position) { emit newPosition(position); });
-        emit newPosition(position);
-    });
 
+    // TODO!! This doesn't work. Check how ArduPilot sets home position.
     Telemetry::Position home = mTelemetry->home();
-    qDebug() << "Home: lat:" << home.latitude_deg
+    qDebug() << "Home: lat:" << home.latitude_deg << "lon:" << home.longitude_deg
-             << "lon:" << home.longitude_deg
+             << "altitude abs:" << home.absolute_altitude_m << "altitude rel:" << home.relative_altitude_m;
-             << "altitude abs:" << home.absolute_altitude_m
-             << "altitude rel:" << home.relative_altitude_m;
 
     return true;
 }
 
-
 bool AzDroneController::stateActionModule(void)
 {
+    // TODO!! Check return value and print warnings and errors.
     mAction = new Action(mSystem);
     if (mAction == nullptr) {
         return false;
@@ -87,55 +97,60 @@ bool AzDroneController::stateActionModule(void)
     return true;
 }
 
-
 bool AzDroneController::stateReadyForArming(void)
 {
-    return mTelemetry->health_all_ok();
+    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)
 {
+    // TODO!! Check return value and print warnings and errors.
     return mAction->arm() == Action::Result::Success;
 }
 
-
 bool AzDroneController::stateTakeoff(void)
 {
+    // TODO!! Drone never reaches the target altitude with ArduPilot. Investigate and fix.
     mAction->set_takeoff_altitude(50);
-    qDebug() << "AzDroneController::stateTakeoff() taking off starts";
-    Action::Result result = mAction->takeoff(); //return mAction->takeoff() == Action::Result::Success;
-    qDebug() << "AzDroneController::stateTakeoff() taking off ends";
 
+    // TODO!! Check return value and print warnings and errors.
+    Action::Result result = mAction->takeoff();
     return result == Action::Result::Success;
 }
 
-
 bool AzDroneController::stateFlyMission(void)
 {
+    // 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)
 {
     qDebug() << "AzDroneController::missionFinishedSlot() Landing the drone.";
+    // TODO!! Check return value and print warnings and errors.
     mAction->land();
-    // TODO!! This must be improved a lot.
+    // TODO!! This must be improved a lot. Disarming should be done when drone lands.
     QTimer::singleShot(20000, this, &AzDroneController::disarmDroneSlot);
 }
 
-
 void AzDroneController::disarmDroneSlot(void)
 {
     qDebug() << "AzDroneController::disarmDroneSlot() Disarming the drone and quit.";
-    // TODO!! This must be improved a lot.
+    // TODO!! Check return value and print warnings and errors.
     mAction->disarm();
+    // TODO!! Check with the team what to do after the landing.
     QCoreApplication::instance()->quit();
 }
 
-
 void AzDroneController::droneStateMachineSlot(void)
 {
     static QString states[] = {
@@ -152,32 +167,33 @@ void AzDroneController::droneStateMachineSlot(void)
     };
 
     qDebug() << "";
-    qDebug() << "AzDroneController::droneStateMachineSlot() Current state:" << states[(int)mDroneState];
+    qDebug() << "AzDroneController::droneStateMachineSlot() Current state:" << states[(int) mDroneState];
 
     using MethodPtr = bool (AzDroneController::*)();
 
-    struct MethodInfo {
+    struct MethodInfo
+    {
         AzDroneState currentState;
         MethodPtr nextStateMethodPtr;
         QString description;
         int nextStateDelay;
     };
 
-    const MethodInfo stateMethods[] = {
+    const MethodInfo stateMethods[]
-        { AZ_DRONE_STATE_DISCONNECTED, &AzDroneController::stateConnect, "stateConnect()", 1000 },
+        = {{AZ_DRONE_STATE_DISCONNECTED, &AzDroneController::stateConnect, "stateConnect()", 1000},
-        { AZ_DRONE_STATE_CONNECTED, &AzDroneController::stateAutopilot, "stateAutopilot()", 1000 },
+           {AZ_DRONE_STATE_CONNECTED, &AzDroneController::stateAutopilot, "stateAutopilot()", 1000},
-        { AZ_DRONE_STATE_AUTOPILOT, &AzDroneController::stateTelemetryModule, "stateTelemetryModule()", 1000 },
+           {AZ_DRONE_STATE_AUTOPILOT, &AzDroneController::stateTelemetryModule, "stateTelemetryModule()", 1000},
-        { AZ_DRONE_STATE_TELEMETRY_MODULE, &AzDroneController::stateActionModule, "stateActionModule()", 1000 },
+           {AZ_DRONE_STATE_TELEMETRY_MODULE, &AzDroneController::stateActionModule, "stateActionModule()", 1000},
-        { AZ_DRONE_STATE_ACTION_MODULE, &AzDroneController::stateReadyForArming, "stateReadyForArming()", 1000 },
+           {AZ_DRONE_STATE_ACTION_MODULE, &AzDroneController::stateReadyForArming, "stateReadyForArming()", 1000},
-        { AZ_DRONE_STATE_READY_FOR_ARMING, &AzDroneController::stateArm, "stateArm()", 1000 },
+           {AZ_DRONE_STATE_READY_FOR_ARMING, &AzDroneController::stateArm, "stateArm()", 1000},
-        { AZ_DRONE_STATE_ARMED, &AzDroneController::stateTakeoff, "stateTakeoff()", 10000 },
+           {AZ_DRONE_STATE_ARMED, &AzDroneController::stateTakeoff, "stateTakeoff()", 10000},
-        { AZ_DRONE_STATE_TAKE_OFF, &AzDroneController::stateFlyMission, "stateFlyMission()", 1000 },
+           {AZ_DRONE_STATE_TAKE_OFF, &AzDroneController::stateFlyMission, "stateFlyMission()", 1000},
-        { AZ_DRONE_STATE_FLY_MISSION, nullptr, "No function to call. AzMissionControl running", 0 }
+           {AZ_DRONE_STATE_FLY_MISSION, nullptr, "No function to call. AzMissionControl running", 0}};
-    };
 
+    // Iterate through states. If state function fails, then it's tried again until it succeeds.
     for (uint i = 0; i < sizeof(stateMethods) / sizeof(MethodInfo); i++) {
         if (stateMethods[i].currentState == mDroneState) {
-            if ((int)mDroneState == (int)AZ_DRONE_STATE_FLY_MISSION) {
+            if ((int) mDroneState == (int) AZ_DRONE_STATE_FLY_MISSION) {
                 qDebug() << "AzMissionContoroller running. Waiting signals from it.";
                 return;
             }
@@ -186,36 +202,35 @@ void AzDroneController::droneStateMachineSlot(void)
 
             if (result) {
                 qDebug() << "AzDroneController::droneStateMachineSlot()" << stateMethods[i].description << "succeeded";
-                qDebug() << "AzDroneController::droneStateMachineSlot() calling next state function in" << stateMethods[i].nextStateDelay << "ms.";
+                qDebug() << "AzDroneController::droneStateMachineSlot() calling next state function in"
+                         << stateMethods[i].nextStateDelay << "ms.";
                 mDroneState = stateMethods[i + 1].currentState;
-                qDebug() << "AzDroneController::droneStateMachineSlot() New state:" << states[(int)mDroneState];
+                qDebug() << "AzDroneController::droneStateMachineSlot() New state:" << states[(int) mDroneState];
             }
             else {
-                qDebug() << "AzDroneController::droneStateMachineSlot() << stateMethods[i].description failed. Trying again.";
+                qDebug() << "AzDroneController::droneStateMachineSlot()" << stateMethods[i].description
+                         << "failed. Trying again.";
             }
 
-            //if (stateMethods[i].nextStateDelay > 0 && stateMethods[i].nextStateMethodPtr != nullptr) {
+            delayedStateCallSlot(stateMethods[i].nextStateDelay);
-                delayedStateCallSlot(stateMethods[i].nextStateDelay);
-            //}
 
             break;
         }
     }
 }
 
-
 void AzDroneController::newPositionSlot(Telemetry::Position position)
 {
     // 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) {
         qDebug() << "AzDroneController::newPositionSlot()"
-                 << "First altitude" << mFirstPosition.relative_altitude_m
+                 << "First altitude" << mFirstPosition.relative_altitude_m << "lat:" << position.latitude_deg
-                 << "lat:" << position.latitude_deg
                  << "lon:" << position.longitude_deg;
         mFirstPosition = position;
     }
 
+    // Send new position to the mission controller.
     if (mDroneState == AZ_DRONE_STATE_FLY_MISSION) {
         mMissionController->newPosition(position);
     }
@@ -225,8 +240,22 @@ void AzDroneController::newPositionSlot(Telemetry::Position position)
     }
 }
 
-
 void AzDroneController::missionIndexChangedSlot(int currentIndex, int totalIndexes)
 {
     qDebug() << "AzDroneController::missionIndexChanged()" << currentIndex << "/" << totalIndexes;
 }
+
+
+void AzDroneController::newHealthInfoSlot(Telemetry::Health health)
+{
+    qDebug() << "AzDroneController::newHealthInfoSlot()";
+    qDebug() << "  Gyro calibration:  " << (health.is_gyrometer_calibration_ok ? "ok" : "not ok");
+    qDebug() << "  Accel calibration: " << (health.is_accelerometer_calibration_ok ? "ok" : "not ok");
+    qDebug() << "  Mag calibration:   " << (health.is_magnetometer_calibration_ok ? "ok" : "not ok");
+    qDebug() << "  Local position:    " << (health.is_local_position_ok ? "ok" : "not ok");
+    qDebug() << "  Global position:   " << (health.is_global_position_ok ? "ok" : "not ok");
+    qDebug() << "  Home position:     " << (health.is_home_position_ok ? "ok" : "not ok");
+
+    // Not optimal, but disconnect signal immeaditely after getting the first update.
+    disconnect(this, &AzDroneController::newHealthInfo, this, &AzDroneController::newHealthInfoSlot);
+}

src/az_drone_controller.h

@@ -1,7 +1,6 @@
 #pragma once
 
 #include <QObject>
-
 #include <memory.h>
 
 #include <mavsdk/mavsdk.h>
@@ -13,7 +12,7 @@
 
 using namespace mavsdk;
 
-typedef enum  {
+typedef enum {
     AZ_DRONE_STATE_DISCONNECTED,
     AZ_DRONE_STATE_CONNECTED,
     AZ_DRONE_STATE_AUTOPILOT,
@@ -32,7 +31,11 @@ class AzDroneController : public QObject
     Q_OBJECT
 public:
     explicit AzDroneController(AzMission &mission, QObject *parent = nullptr);
+
+    // Starts the autopilot.
     void start();
+
+    // Specify the methods that will be called one at a time to initialise the drone and fly the mission.
     bool stateConnect(void);
     bool stateAutopilot(void);
     bool stateTelemetryModule(void);
@@ -42,17 +45,41 @@ public:
     bool stateTakeoff(void);
     bool stateFlyMission(void);
 
+    // Slots that are called by the emitted Qt signals.
 private slots:
+    // Launches a state machine that progressively initialises the drone without blocking.
     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);
+
+    // Disarms the drone and exits the application. TODO!! Discuss quitting the application.
     void disarmDroneSlot(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);
+
 private:
     Mavsdk mMavsdk;
     AzDroneState mDroneState;

src/az_mission.cpp

@@ -1,5 +1,3 @@
-#include "az_mission.h"
-
 #include <filesystem>
 #include <iostream>
 #include <string>
@@ -8,13 +6,15 @@
 #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) :
+AzMission::AzMission(string filename)
-    mFilename(filename),
+    : mFilename(filename)
-    mOperationalHeight(INVALID_HEIGHT),
+    , mOperationalHeight(INVALID_HEIGHT)
-    mReturnPoint(AzCoordinate(INVALID_LATITUDE, INVALID_LONGITUDE))
+    , mReturnPoint(AzCoordinate(INVALID_LATITUDE, INVALID_LONGITUDE))
 {
     if (filesystem::exists(mFilename) == false) {
         cerr << "JSON file " << mFilename << " doesn't exists." << endl;

src/az_mission_controller.cpp

@@ -1,26 +1,25 @@
 #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) :
+AzMissionController::AzMissionController(AzMission &mission, QObject *parent)
-    QObject(parent),
+    : QObject(parent)
-    mMission(mission),
+    , mMission(mission)
-    mAction(nullptr),
+    , mAction(nullptr)
-    mMissionStarted(false),
+    , mMissionStarted(false)
-    mAbsoluteAltitude(-10000)
+    , mAbsoluteAltitude(-10000)
-{
+{}
-}
-
 
 void AzMissionController::setAction(Action *action)
 {
     mAction = action;
 }
 
-
 bool AzMissionController::startMissions(float absoluteAltitude)
 {
     mCurrentMissionIndex = -1;
@@ -29,36 +28,35 @@ bool AzMissionController::startMissions(float absoluteAltitude)
     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()) {
+    if (mCurrentMissionIndex >= (int) mMission.getActionPoints().size()) {
-        qDebug() << "AzMissionController::flyToNextMissionItem() All mission items are handled. Stopping mission handling.";
+        qDebug() << "AzMissionController::flyToNextMissionItem() All mission action points are handled.";
         mMissionStarted = false;
         emit finished();
         return true;
     }
 
-    qDebug() << "AzMissionController::flyToNextMissionItem() flying to the item"
+    qDebug() << "AzMissionController::flyToNextMissionItem() flying to the item" << mCurrentMissionIndex + 1 << "/"
-             << mCurrentMissionIndex + 1 << "/" << mMission.getActionPoints().size();
+             << mMission.getActionPoints().size();
 
     const AzCoordinate &coordinate = mMission.getActionPoints().at(mCurrentMissionIndex).getPoint();
-    qDebug() << "AzMissionController::flyToNextMissionItem() navigating to point"
+    qDebug() << "AzMissionController::flyToNextMissionItem() navigating to point" << coordinate.latitude
-             << coordinate.latitude << coordinate.longitude;
+             << 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";
 
+    // TODO!! Check return value and print warnings and errors.
     if (result == Action::Result::Success) {
         emit missionProgressChanged(mCurrentMissionIndex + 1, mMission.getActionPoints().size());
         return true;
@@ -67,25 +65,19 @@ bool AzMissionController::flyToNextMissionItem(void)
     return false;
 }
 
-
+void AzMissionController::newPosition(Telemetry::Position pos)
-void AzMissionController::newPosition(Telemetry::Position position)
 {
-    (void)position;
-
     if (mMissionStarted == false) {
         return;
     }
 
-    const AzCoordinate &coordinate = mMission.getActionPoints().at(mCurrentMissionIndex).getPoint();
+    const AzCoordinate &target = mMission.getActionPoints().at(mCurrentMissionIndex).getPoint();
-    float distanceToTarget = AzUtils::distance(position.latitude_deg,
+    float distance = AzUtils::distance(pos.latitude_deg, pos.longitude_deg, target.latitude, target.longitude);
-                                               position.longitude_deg,
-                                               coordinate.latitude,
-                                               coordinate.longitude);
 
-    qDebug() << "AzMissionController::newPosition() distanceToTarget: " << distanceToTarget;
+    qDebug() << "AzMissionController::newPosition() distance to target: " << distance;
 
-    // TODO!! In final application we need to
+    // TODO!! In final application we need to use the camera to find the target.
-    if (distanceToTarget <= 0.01) {
+    if (distance <= 0.01) {
         qDebug() << "AzMissionController::newPosition() target reached. Continuing to the next item.";
         flyToNextMissionItem();
     }

src/az_mission_controller.h

@@ -1,7 +1,9 @@
 #pragma once
 
 #include <QObject>
+
 #include <mavsdk/plugins/telemetry/telemetry.h>
+
 #include "az_mission.h"
 #include "plugins/action/action.h"
 

src/az_utils.cpp

@@ -5,14 +5,17 @@ AzUtils::AzUtils() {}
 
 #define EARTH_RADIUS 6371.0 // Earth radius in kilometers
 
-double degrees_to_radians(double degrees) {
+double degrees_to_radians(double degrees)
+{
     return degrees * M_PI / 180.0;
 }
 
-double AzUtils::distance(double lat1, double lon1, double lat2, double lon2) {
+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 a    = sin(dlat / 2) * sin(dlat / 2)
-    double c = 2 * atan2(sqrt(a), sqrt(1-a));
+               + 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;
 }

src/main.cpp

@@ -1,20 +1,24 @@
 #include <QCoreApplication>
 #include <QDebug>
-#include "az_mission.h"
+
 #include "az_drone_controller.h"
+#include "az_mission.h"
 
 int main(int argc, char *argv[])
 {
+    // This is needed to have main event loop and signal-slot events in the AzDroneController.
     QCoreApplication a(argc, argv);
 
     if (argc != 2) {
-        qCritical() << "Please give mission file as an argument\n";
+        qCritical() << "Please give mission JSON file as an argument.\n";
         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.
     AzDroneController droneController(mission);
     droneController.start();