Added calibration plugin

This adds a calibration plugin which right now supports gyro,
accelerometer, and magnetometer calibration.

cmake/unit_tests.cmake

@@ -16,6 +16,7 @@ target_link_libraries(unit_tests_runner
     dronecore
     dronecore_mission
     dronecore_camera
+    dronecore_calibration
     gtest
     gtest_main
     gmock

core/mavlink_commands.h

@@ -49,6 +49,7 @@ class MAVLinkCommands {
 
         // In some cases "Reserved" value could be "0".
         // This utility method can be used in such case.
+        // TODO: rename to set_all
         static void set_as_reserved(Params &params, float reserved_value = NAN)
         {
             params.param1 = reserved_value;
@@ -76,6 +77,7 @@ class MAVLinkCommands {
             float param7 = NAN;
         } params{};
 
+        // TODO: rename to set_all
         static void set_as_reserved(Params &params, float reserved_value = NAN)
         {
             params.param1 = reserved_value;

integration_tests/CMakeLists.txt

@@ -32,6 +32,7 @@ add_executable(integration_tests_runner
     transition_multicopter_fixedwing.cpp
     follow_me.cpp
     multi_components.cpp
+    calibration.cpp
     camera_take_photo.cpp
     camera_take_photo_interval.cpp
     camera_mode.cpp
@@ -60,6 +61,7 @@ target_link_libraries(integration_tests_runner
     dronecore_gimbal
     dronecore_follow_me
     dronecore_camera
+    dronecore_calibration
     gtest
     gtest_main
     gmock

integration_tests/calibration.cpp

@@ -0,0 +1,107 @@
+#include <iostream>
+#include "integration_test_helper.h"
+#include "global_include.h"
+#include "dronecore.h"
+#include "plugins/calibration/calibration.h"
+
+using namespace dronecore;
+using namespace std::placeholders; // for `_1`
+
+static void receive_calibration_callback(Calibration::Result result,
+                                         float progress,
+                                         const std::string text,
+                                         const std::string calibration_type,
+                                         bool &done);
+
+TEST(HardwareTest, GyroCalibration)
+{
+    DroneCore dc;
+
+    ConnectionResult ret = dc.add_serial_connection("/dev/ttyACM0");
+    ASSERT_EQ(ret, ConnectionResult::SUCCESS);
+
+    // Wait for system to connect via heartbeat.
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+    System &system = dc.system();
+
+    auto calibration = std::make_shared<Calibration>(system);
+
+    bool done = false;
+
+    calibration->calibrate_gyro_async(
+        std::bind(&receive_calibration_callback, _1, _2, _3, "gyro", std::ref(done)));
+
+    while (!done) {
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+}
+
+TEST(HardwareTest, AccelerometerCalibration)
+{
+    DroneCore dc;
+
+    ConnectionResult ret = dc.add_serial_connection("/dev/ttyACM0");
+    ASSERT_EQ(ret, ConnectionResult::SUCCESS);
+
+    // Wait for system to connect via heartbeat.
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+    System &system = dc.system();
+
+    auto calibration = std::make_shared<Calibration>(system);
+
+    bool done = false;
+
+    calibration->calibrate_accelerometer_async(
+        std::bind(&receive_calibration_callback, _1, _2, _3, "accelerometer", std::ref(done)));
+
+    while (!done) {
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+}
+
+TEST(HardwareTest, MagnetometerCalibration)
+{
+    DroneCore dc;
+
+    ConnectionResult ret = dc.add_serial_connection("/dev/ttyACM0");
+    ASSERT_EQ(ret, ConnectionResult::SUCCESS);
+
+    // Wait for system to connect via heartbeat.
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+    System &system = dc.system();
+
+    auto calibration = std::make_shared<Calibration>(system);
+
+    bool done = false;
+
+    calibration->calibrate_magnetometer_async(
+        std::bind(&receive_calibration_callback, _1, _2, _3, "magnetometer", std::ref(done)));
+
+    while (!done) {
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+}
+
+void receive_calibration_callback(Calibration::Result result,
+                                  float progress,
+                                  const std::string text,
+                                  const std::string calibration_type,
+                                  bool &done)
+{
+    if (result == Calibration::Result::IN_PROGRESS) {
+        LogInfo() << calibration_type << " calibration in progress: " << progress;
+    } else if (result == Calibration::Result::INSTRUCTION) {
+        LogInfo() << calibration_type << " calibration instruction: " << text;
+    } else {
+        EXPECT_EQ(result, Calibration::Result::SUCCESS);
+
+        if (result != Calibration::Result::SUCCESS) {
+            LogErr() << calibration_type
+                     << " calibration error: " << Calibration::result_str(result);
+            if (result == Calibration::Result::FAILED) {
+                LogErr() << calibration_type << " cailbration failed: " << text;
+            }
+        }
+        done = true;
+    }
+}

plugins/CMakeLists.txt

@@ -18,5 +18,6 @@ add_subdirectory(logging)
 add_subdirectory(info)
 add_subdirectory(follow_me)
 add_subdirectory(camera)
+add_subdirectory(calibration)
 
 set(UNIT_TEST_SOURCES ${UNIT_TEST_SOURCES} PARENT_SCOPE)

plugins/calibration/CMakeLists.txt

@@ -0,0 +1,24 @@
+add_library(dronecore_calibration ${PLUGIN_LIBRARY_TYPE}
+    calibration.cpp
+    calibration_impl.cpp
+    calibration_statustext_parser.cpp
+)
+
+target_link_libraries(dronecore_calibration
+    dronecore
+)
+
+install(FILES
+    calibration.h
+    DESTINATION ${dronecore_install_include_dir}
+)
+
+install(TARGETS dronecore_calibration
+    #EXPORT dronecore-targets
+    DESTINATION ${dronecore_install_lib_dir}
+)
+
+list(APPEND UNIT_TEST_SOURCES
+    ${CMAKE_CURRENT_SOURCE_DIR}/calibration_statustext_parser_test.cpp
+)
+set(UNIT_TEST_SOURCES ${UNIT_TEST_SOURCES} PARENT_SCOPE)

plugins/calibration/calibration.cpp

@@ -0,0 +1,61 @@
+#include "calibration.h"
+#include "calibration_impl.h"
+#include "calibration_messages.h"
+
+namespace dronecore {
+
+Calibration::Calibration(System &system) : PluginBase()
+{
+    _impl = new CalibrationImpl(system);
+}
+
+Calibration::~Calibration()
+{
+    delete _impl;
+}
+
+void Calibration::calibrate_gyro_async(calibration_callback_t callback)
+{
+    _impl->calibrate_gyro_async(callback);
+}
+
+void Calibration::calibrate_accelerometer_async(calibration_callback_t callback)
+{
+    _impl->calibrate_accelerometer_async(callback);
+}
+
+void Calibration::calibrate_magnetometer_async(calibration_callback_t callback)
+{
+    _impl->calibrate_magnetometer_async(callback);
+}
+
+const char *Calibration::result_str(Result result)
+{
+    switch (result) {
+        case Result::SUCCESS:
+            return "Success";
+        case Result::IN_PROGRESS:
+            return "In progress";
+        case Result::INSTRUCTION:
+            return "Instruction";
+        case Result::FAILED:
+            return "Failed";
+        case Result::NO_SYSTEM:
+            return "No system";
+        case Result::CONNECTION_ERROR:
+            return "Connection error";
+        case Result::BUSY:
+            return "Busy";
+        case Result::COMMAND_DENIED:
+            return "Command denied";
+        case Result::TIMEOUT:
+            return "Timeout";
+        case Result::CANCELLED:
+            return "CANCELLED";
+        case Result::UNKNOWN:
+        default:
+            return "Unknown";
+    }
+}
+
+} // namespace dronecore

plugins/calibration/calibration.h

@@ -0,0 +1,98 @@
+#pragma once
+
+#include <functional>
+#include "plugin_base.h"
+
+namespace dronecore {
+
+class CalibrationImpl;
+class System;
+
+class Calibration : public PluginBase {
+public:
+    /**
+     * @brief Constructor. Creates the plugin for a specific System.
+     *
+     * The plugin is typically created as shown below:
+     *
+     *     ```cpp
+     *     auto calibration = std::make_shared<Calibration>(system);
+     *     ```
+     *
+     * @param system The specific system associated with this plugin.
+     */
+    explicit Calibration(System &system);
+
+    /**
+     * @brief Destructor (internal use only).
+     */
+    ~Calibration();
+
+    /**
+     * @brief Possible results returned for camera commands.
+     */
+    enum class Result {
+        UNKNOWN,
+        SUCCESS,
+        IN_PROGRESS,
+        INSTRUCTION,
+        FAILED,
+        NO_SYSTEM,
+        CONNECTION_ERROR,
+        BUSY,
+        COMMAND_DENIED,
+        TIMEOUT,
+        CANCELLED
+    };
+
+    /**
+     * @brief Returns a human-readable English string for Calibration::Result.
+     *
+     * @param result The enum value for which a human readable string is required.
+     * @return Human readable string for the Calibration::Result.
+     */
+    static const char *result_str(Result result);
+
+    /**
+     * @brief Callback type for asynchronous calibration call.
+     */
+    typedef std::function<void(Result result, float progress, const std::string &text)>
+        calibration_callback_t;
+
+    /**
+     * @brief Perform gyro calibration (asynchronous call).
+     *
+     * @param callback Function to receive result and progress of calibration.
+     */
+    void calibrate_gyro_async(calibration_callback_t callback);
+
+    /**
+     * @brief Perform accelerometer calibration (asynchronous call).
+     *
+     * @param callback Function to receive result and progress of calibration.
+     */
+    void calibrate_accelerometer_async(calibration_callback_t callback);
+
+    /**
+     * @brief Perform magnetometer calibration (asynchronous call).
+     *
+     * @param callback Function to receive result and progress of calibration.
+     */
+    void calibrate_magnetometer_async(calibration_callback_t callback);
+
+    /**
+     * @brief Copy constructor (object is not copyable).
+     */
+    Calibration(const Calibration &) = delete;
+
+    /**
+     * @brief Equality operator (object is not copyable).
+     */
+    const Calibration &operator=(const Calibration &) = delete;
+
+private:
+    /** @private Underlying implementation, set at instantiation */
+    CalibrationImpl *_impl;
+};
+
+} // namespace dronecore