From ab791eb254ece0cd1cfec125daf0369f7219500d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tuomas=20J=C3=A4rvinen?= <jarvinentuomas@protonmail.com>
Date: Mon, 20 May 2024 21:02:32 +0200
Subject: [PATCH] Improve use of MAVSDK::add_any_connection()

Added error printing and possibility to use UART as connection
to the flight controller.

Issue: https://denyspopov.atlassian.net/browse/AZ-16
Type: New Feature
---
 src/az_config.h               |  5 ++---
 src/az_drone_controller.cpp   | 27 ++++++++++++++++++++-------
 src/az_drone_controller.h     |  2 +-
 src/az_mission_controller.cpp |  3 +--
 src/az_utils.cpp              |  2 +-
 src/main.cpp                  |  5 +++--
 6 files changed, 28 insertions(+), 16 deletions(-)

diff --git a/src/az_config.h b/src/az_config.h
index 5de2d2c..24d109e 100644
--- a/src/az_config.h
+++ b/src/az_config.h
@@ -1,6 +1,5 @@
 #pragma once
 
-#define FUNCTION_NAME(func) func()
-
-const char *AZ_CONNECTION          = "udp://:14550";
+const char *AZ_CONNECTION_SERIAL   = "serial:///dev/ttyS0:115200";
+const char *AZ_CONNECTION_UDP      = "udp://:14550";
 const int AZ_RELATIVE_FLY_ALTITUDE = 50;
diff --git a/src/az_drone_controller.cpp b/src/az_drone_controller.cpp
index 62e25dc..f48968a 100644
--- a/src/az_drone_controller.cpp
+++ b/src/az_drone_controller.cpp
@@ -48,11 +48,27 @@ void AzDroneController::delayedStateCallSlot(int ms)
     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)
 {
-    // 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;
+    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 {
+        std::cerr << "MAVSDK::add_any_connection() failed. Reason: " << result << endl;
+        return false;
+    }
 }
 
 bool AzDroneController::stateAutopilot(void)
@@ -104,9 +120,7 @@ bool AzDroneController::stateReadyForArming(void)
     bool result = mTelemetry->health_all_ok();
 
     if (result == false) {
-        mTelemetry->subscribe_health([this](Telemetry::Health health) {
-            emit newHealthInfo(health);
-        });
+        mTelemetry->subscribe_health([this](Telemetry::Health health) { emit newHealthInfo(health); });
     }
 
     return result;
@@ -247,7 +261,6 @@ void AzDroneController::missionIndexChangedSlot(int currentIndex, int totalIndex
     qDebug() << "AzDroneController::missionIndexChanged()" << currentIndex << "/" << totalIndexes;
 }
 
-
 void AzDroneController::newHealthInfoSlot(Telemetry::Health health)
 {
     qDebug() << "AzDroneController::newHealthInfoSlot()";
diff --git a/src/az_drone_controller.h b/src/az_drone_controller.h
index 326661d..e30c770 100644
--- a/src/az_drone_controller.h
+++ b/src/az_drone_controller.h
@@ -1,7 +1,7 @@
 #pragma once
 
-#include <QObject>
 #include <memory.h>
+#include <QObject>
 
 #include <mavsdk/mavsdk.h>
 #include <mavsdk/plugins/action/action.h>
diff --git a/src/az_mission_controller.cpp b/src/az_mission_controller.cpp
index cfdf917..6e6a32f 100644
--- a/src/az_mission_controller.cpp
+++ b/src/az_mission_controller.cpp
@@ -2,11 +2,10 @@
 
 #include <mavsdk/plugins/telemetry/telemetry.h>
 
-#include "az_mission_controller.h"
 #include "az_mission.h"
+#include "az_mission_controller.h"
 #include "az_utils.h"
 
-
 AzMissionController::AzMissionController(AzMission &mission, QObject *parent)
     : QObject(parent)
     , mMission(mission)
diff --git a/src/az_utils.cpp b/src/az_utils.cpp
index 480f16b..4303111 100644
--- a/src/az_utils.cpp
+++ b/src/az_utils.cpp
@@ -1,5 +1,5 @@
-#include <math.h>
 #include "az_utils.h"
+#include <math.h>
 
 AzUtils::AzUtils() {}
 
diff --git a/src/main.cpp b/src/main.cpp
index 74f70ee..c723c3a 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -9,8 +9,9 @@ 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 JSON file as an argument.\n";
+    if (argc < 2) {
+        qCritical() << "\nPass the mission JSON file as the first argument.";
+        qCritical() << "A serial port connection can be enabled with \"serial\" as the second argument.\n";
         return 1;
     }