camera: support multiple cameras within one instance

This changes the API to support more than one camera within one
camera plugin instance.

This will enable multiple cameras in language wrappers instead of just
C++ as it is now.

Work in progress ...

src/mavsdk/core/CMakeLists.txt

@@ -25,7 +25,7 @@ target_sources(mavsdk
     flight_mode.cpp
     fs_utils.cpp
     inflate_lzma.cpp
-    math_conversions.cpp
+    math_utils.cpp
     mavsdk.cpp
     mavsdk_impl.cpp
     mavlink_channels.cpp
@@ -62,6 +62,7 @@ target_sources(mavsdk
     cli_arg.cpp
     geometry.cpp
     mavsdk_time.cpp
+    string_utils.cpp
     timesync.cpp
 )
 
@@ -185,8 +186,7 @@ list(APPEND UNIT_TEST_SOURCES
     ${PROJECT_SOURCE_DIR}/mavsdk/core/file_cache_test.cpp
     ${PROJECT_SOURCE_DIR}/mavsdk/core/locked_queue_test.cpp
     ${PROJECT_SOURCE_DIR}/mavsdk/core/geometry_test.cpp
-    ${PROJECT_SOURCE_DIR}/mavsdk/core/math_conversions_test.cpp
-    ${PROJECT_SOURCE_DIR}/mavsdk/core/mavsdk_math_test.cpp
+    ${PROJECT_SOURCE_DIR}/mavsdk/core/math_utils_test.cpp
     ${PROJECT_SOURCE_DIR}/mavsdk/core/mavsdk_test.cpp
     ${PROJECT_SOURCE_DIR}/mavsdk/core/mavsdk_time_test.cpp
     ${PROJECT_SOURCE_DIR}/mavsdk/core/mavlink_channels_test.cpp
@@ -199,5 +199,6 @@ list(APPEND UNIT_TEST_SOURCES
     ${PROJECT_SOURCE_DIR}/mavsdk/core/timeout_handler_test.cpp
     ${PROJECT_SOURCE_DIR}/mavsdk/core/unittests_main.cpp
     ${PROJECT_SOURCE_DIR}/mavsdk/core/mavlink_parameter_cache_test.cpp
+    ${PROJECT_SOURCE_DIR}/mavsdk/core/string_utils_test.cpp
 )
 set(UNIT_TEST_SOURCES ${UNIT_TEST_SOURCES} PARENT_SCOPE)

src/mavsdk/core/curl_wrapper.cpp

@@ -76,14 +76,14 @@ static int download_progress_update(
     }
 
     if (dltotal == 0 || dlnow == 0) {
-        return myp->progress_callback(0, HttpStatus::Idle, CURLcode::CURLE_OK);
-    }
-
-    int percentage = static_cast<int>(100 / dltotal * dlnow);
+        myp->progress_callback(0, HttpStatus::Idle, CURLcode::CURLE_OK);
+    } else {
+        int percentage = static_cast<int>(100 * dlnow / dltotal);
 
-    if (percentage > myp->progress_in_percentage) {
-        myp->progress_in_percentage = percentage;
-        return myp->progress_callback(percentage, HttpStatus::Downloading, CURLcode::CURLE_OK);
+        if (percentage > myp->progress_in_percentage) {
+            myp->progress_in_percentage = percentage;
+            myp->progress_callback(percentage, HttpStatus::Downloading, CURLcode::CURLE_OK);
+        }
     }
 
     return 0;

src/mavsdk/core/curl_wrapper_types.h

@@ -6,7 +6,7 @@ namespace mavsdk {
 
 enum class HttpStatus { Idle = 0, Downloading = 1, Uploading = 2, Finished = 3, Error = 4 };
 
-using ProgressCallback = std::function<int(int progress, HttpStatus status, CURLcode curl_code)>;
+using ProgressCallback = std::function<void(int progress, HttpStatus status, CURLcode curl_code)>;
 
 struct UpProgress {
     int progress_in_percentage = 0;

src/mavsdk/core/fs_utils.cpp

@@ -1,8 +1,10 @@
 
 #include "fs_utils.h"
 #include "log.h"
+#include <cctype>
 #include <fstream>
 #include <random>
+#include <string>
 
 #if defined(LINUX) || defined(APPLE)
 #include <pwd.h>
@@ -112,4 +114,17 @@ std::optional<std::filesystem::path> create_tmp_directory(const std::string& pre
     LogErr() << "Could not create a temporary directory, aborting.";
     return std::nullopt;
 }
+
+std::string replace_non_ascii_and_whitespace(const std::string& input)
+{
+    std::string result = input;
+    std::transform(result.begin(), result.end(), result.begin(), [](const char ch) {
+        if (ch < 0 || static_cast<unsigned char>(ch) > 127 || std::isspace(ch)) {
+            return '_';
+        }
+        return ch;
+    });
+    return result;
+}
+
 } // namespace mavsdk

src/mavsdk/core/fs_utils.h

@@ -17,4 +17,6 @@ std::optional<std::filesystem::path> get_cache_directory();
  */
 std::optional<std::filesystem::path> create_tmp_directory(const std::string& prefix);
 
+std::string replace_non_ascii_and_whitespace(const std::string& input);
+
 } // namespace mavsdk

src/mavsdk/core/geometry.cpp

@@ -1,5 +1,5 @@
 #include "geometry.h"
-#include "mavsdk_math.h"
+#include "math_utils.h"
 #include <cmath>
 
 namespace mavsdk::geometry {

src/mavsdk/core/http_loader.cpp

@@ -46,9 +46,7 @@ bool HttpLoader::download_sync(const std::string& url, const std::string& local_
 }
 
 void HttpLoader::download_async(
-    const std::string& url,
-    const std::string& local_path,
-    const ProgressCallback& progress_callback)
+    const std::string& url, const std::string& local_path, ProgressCallback progress_callback)
 {
     auto work_item = std::make_shared<DownloadItem>(url, local_path, progress_callback);
     _work_queue.enqueue(work_item);

src/mavsdk/core/http_loader.h

@@ -29,13 +29,7 @@ class HttpLoader {
     void download_async(
         const std::string& url,
         const std::string& local_path,
-        const ProgressCallback& progress_callback = nullptr);
-
-    bool upload_sync(const std::string& target_url, const std::string& local_path);
-    void upload_async(
-        const std::string& target_url,
-        const std::string& local_path,
-        const ProgressCallback& progress_callback = nullptr);
+        ProgressCallback progress_callback = nullptr);
 
     // Non-copyable
     HttpLoader(const HttpLoader&) = delete;

src/mavsdk/core/math_conversions.cpp → src/mavsdk/core/math_utils.cpp

@@ -1,5 +1,4 @@
-#include "mavsdk_math.h"
-#include "math_conversions.h"
+#include "math_utils.h"
 #include <cmath>
 
 namespace mavsdk {

src/mavsdk/core/mavsdk_math.h → src/mavsdk/core/math_utils.h

@@ -2,6 +2,27 @@
 
 namespace mavsdk {
 
+struct Quaternion {
+    float w;
+    float x;
+    float y;
+    float z;
+};
+
+struct EulerAngle {
+    float roll_deg;
+    float pitch_deg;
+    float yaw_deg;
+};
+
+bool operator==(const Quaternion& lhs, const Quaternion& rhs);
+bool operator==(const EulerAngle& lhs, const EulerAngle& rhs);
+
+EulerAngle to_euler_angle_from_quaternion(Quaternion quaternion);
+Quaternion to_quaternion_from_euler_angle(EulerAngle euler_angle);
+
+Quaternion operator*(const Quaternion& lhs, const Quaternion& rhs);
+
 // Instead of using the constant from math.h or cmath we define it ourselves. This way
 // we don't import all the other C math functions and make sure to use the C++ functions
 // from the standard library (e.g. std::abs() instead of abs()).

src/mavsdk/core/math_conversions_test.cpp → src/mavsdk/core/math_utils_test.cpp

@@ -1,11 +1,15 @@
-#include "math_conversions.h"
-#include <gtest/gtest.h>
+#include "math_utils.h"
 #include "mavlink_include.h"
-#include "mavsdk_math.h"
+#include <gtest/gtest.h>
+#include <cmath>
 
 using namespace mavsdk;
 
-TEST(MathConversions, QuaternionToEulerAnglesAndBackBaseCase)
+#ifndef M_PI_F
+#define M_PI_F static_cast<float>(M_PI)
+#endif
+
+TEST(MathUtils, QuaternionToEulerAnglesAndBackBaseCase)
 {
     Quaternion q1;
     q1.w = 1.0f;
@@ -56,7 +60,7 @@ TEST(MathConversions, QuaternionToEulerAnglesAndBackBaseCase)
 // Example taken from https://quaternions.online/ with Euler angle ZYX-order.
 // The accuracy was not great and I had to increase the test margins quite a
 // bit to make the test pass. I'm not sure where the errors come from.
-TEST(MathConversions, QuaternionToEulerAndBackSomeCase)
+TEST(MathUtils, QuaternionToEulerAndBackSomeCase)
 {
     Quaternion q1;
     q1.w = 0.143f;
@@ -107,7 +111,7 @@ TEST(MathConversions, QuaternionToEulerAndBackSomeCase)
     EXPECT_NEAR(q2.z, q2_mavlink[3], 0.01f);
 }
 
-TEST(MathConversions, QuaternionRotation)
+TEST(MathUtils, QuaternionRotation)
 {
     // Define a sample quaternion
     auto sample_q = to_quaternion_from_euler_angle(EulerAngle{30.f, 60.f, -45.f});
@@ -125,3 +129,39 @@ TEST(MathConversions, QuaternionRotation)
     EXPECT_NEAR(expected_q.y, rotated_q.y, 1e-3);
     EXPECT_NEAR(expected_q.z, rotated_q.z, 1e-3);
 }
+
+
+TEST(MathUtils, OurPi)
+{
+    ASSERT_DOUBLE_EQ(PI, M_PI);
+}
+
+TEST(MathUtils, RadDegDouble)
+{
+    ASSERT_DOUBLE_EQ(0.0, to_rad_from_deg(0.0));
+    ASSERT_DOUBLE_EQ(M_PI / 2.0, to_rad_from_deg(90.0));
+    ASSERT_DOUBLE_EQ(M_PI, to_rad_from_deg(180.0));
+    ASSERT_DOUBLE_EQ(-M_PI, to_rad_from_deg(-180.0));
+    ASSERT_DOUBLE_EQ(2.0 * M_PI, to_rad_from_deg(360.0));
+
+    ASSERT_DOUBLE_EQ(0.0, to_deg_from_rad(0.0));
+    ASSERT_DOUBLE_EQ(90.0, to_deg_from_rad(M_PI / 2.0));
+    ASSERT_DOUBLE_EQ(180, to_deg_from_rad(M_PI));
+    ASSERT_DOUBLE_EQ(-180, to_deg_from_rad(-M_PI));
+    ASSERT_DOUBLE_EQ(360.0, to_deg_from_rad(2.0 * M_PI));
+}
+
+TEST(MathUtils, RadDegFloat)
+{
+    ASSERT_FLOAT_EQ(0.0f, to_rad_from_deg(0.0f));
+    ASSERT_FLOAT_EQ(M_PI_F / 2.0f, to_rad_from_deg(90.0f));
+    ASSERT_FLOAT_EQ(M_PI_F, to_rad_from_deg(180.0f));
+    ASSERT_FLOAT_EQ(-M_PI_F, to_rad_from_deg(-180.0f));
+    ASSERT_FLOAT_EQ(2.0f * M_PI_F, to_rad_from_deg(360.0f));
+
+    ASSERT_FLOAT_EQ(0.0f, to_deg_from_rad(0.0f));
+    ASSERT_FLOAT_EQ(90.0f, to_deg_from_rad(M_PI_F / 2.0f));
+    ASSERT_FLOAT_EQ(180.0f, to_deg_from_rad(M_PI_F));
+    ASSERT_FLOAT_EQ(-180.0f, to_deg_from_rad(-M_PI_F));
+    ASSERT_FLOAT_EQ(360.0f, to_deg_from_rad(2.0f * M_PI_F));
+}

src/mavsdk/core/mavlink_command_sender.cpp

@@ -36,49 +36,55 @@ MavlinkCommandSender::~MavlinkCommandSender()
     }
 }
 
-MavlinkCommandSender::Result
-MavlinkCommandSender::send_command(const MavlinkCommandSender::CommandInt& command)
+MavlinkCommandSender::Result MavlinkCommandSender::send_command(
+    const MavlinkCommandSender::CommandInt& command, unsigned retries)
 {
     // We wrap the async call with a promise and future.
     auto prom = std::make_shared<std::promise<Result>>();
     auto res = prom->get_future();
 
-    queue_command_async(command, [prom](Result result, float progress) {
-        UNUSED(progress);
-        // We can only fulfill the promise once in C++11.
-        // Therefore, we have to ignore the IN_PROGRESS state and wait
-        // for the final result.
-        if (result != Result::InProgress) {
-            prom->set_value(result);
-        }
-    });
+    queue_command_async(
+        command,
+        [prom](Result result, float progress) {
+            UNUSED(progress);
+            // We can only fulfill the promise once in C++11.
+            // Therefore, we have to ignore the IN_PROGRESS state and wait
+            // for the final result.
+            if (result != Result::InProgress) {
+                prom->set_value(result);
+            }
+        },
+        retries);
 
     // Block now to wait for result.
     return res.get();
 }
 
-MavlinkCommandSender::Result
-MavlinkCommandSender::send_command(const MavlinkCommandSender::CommandLong& command)
+MavlinkCommandSender::Result MavlinkCommandSender::send_command(
+    const MavlinkCommandSender::CommandLong& command, unsigned retries)
 {
     // We wrap the async call with a promise and future.
     auto prom = std::make_shared<std::promise<Result>>();
     auto res = prom->get_future();
 
-    queue_command_async(command, [prom](Result result, float progress) {
-        UNUSED(progress);
-        // We can only fulfill the promise once in C++11.
-        // Therefore, we have to ignore the IN_PROGRESS state and wait
-        // for the final result.
-        if (result != Result::InProgress) {
-            prom->set_value(result);
-        }
-    });
+    queue_command_async(
+        command,
+        [prom](Result result, float progress) {
+            UNUSED(progress);
+            // We can only fulfill the promise once in C++11.
+            // Therefore, we have to ignore the IN_PROGRESS state and wait
+            // for the final result.
+            if (result != Result::InProgress) {
+                prom->set_value(result);
+            }
+        },
+        retries);
 
     return res.get();
 }
 
 void MavlinkCommandSender::queue_command_async(
-    const CommandInt& command, const CommandResultCallback& callback)
+    const CommandInt& command, const CommandResultCallback& callback, unsigned retries)
 {
     if (_command_debugging) {
         LogDebug() << "COMMAND_INT " << static_cast<int>(command.command) << " to send to "
@@ -103,11 +109,12 @@ void MavlinkCommandSender::queue_command_async(
     new_work->command = command;
     new_work->identification = identification;
     new_work->callback = callback;
+    new_work->retries_to_do = retries;
     _work_queue.push_back(new_work);
 }
 
 void MavlinkCommandSender::queue_command_async(
-    const CommandLong& command, const CommandResultCallback& callback)
+    const CommandLong& command, const CommandResultCallback& callback, unsigned retries)
 {
     if (_command_debugging) {
         LogDebug() << "COMMAND_LONG " << static_cast<int>(command.command) << " to send to "
@@ -133,6 +140,7 @@ void MavlinkCommandSender::queue_command_async(
     new_work->identification = identification;
     new_work->callback = callback;
     new_work->time_started = _system_impl.get_time().steady_time();
+    new_work->retries_to_do = retries;
     _work_queue.push_back(new_work);
 }
 
@@ -358,12 +366,12 @@ void MavlinkCommandSender::receive_timeout(const CommandIdentification& identifi
                     LogErr() << "No command, that's awkward";
                     continue;
                 }
-                LogErr() << "Retrying failed for REQUEST_MESSAGE command for message: "
-                         << work->identification.maybe_param1 << ", to ("
-                         << std::to_string(target_sysid) << "/" << std::to_string(target_compid)
-                         << ")";
+                LogWarn() << "Retrying failed for REQUEST_MESSAGE command for message: "
+                          << work->identification.maybe_param1 << ", to ("
+                          << std::to_string(target_sysid) << "/" << std::to_string(target_compid)
+                          << ")";
             } else {
-                LogErr() << "Retrying failed for command: " << work->identification.command << ")";
+                LogWarn() << "Retrying failed for command: " << work->identification.command << ")";
             }
 
             temp_callback = work->callback;