Full gimbal angle control (#2210)

* Added gimbal function SetAngles

* Updated proto

* Fixed typo

* cleaned up code

* Compiler option added for raspberry pi

* Generated files to compile

* Added new test for the gimbal

* Added roll angle to output

* Fixed typo

* Changed order of angels to MAVLink order

* updated protos

* Reverted submodule to mavlink repo

* Update proto submodule, fix style

Signed-off-by: Julian Oes <[email protected]>

* examples: improve description

Worth mentioning what the example is about.

Signed-off-by: Julian Oes <[email protected]>

---------

Signed-off-by: Julian Oes <[email protected]>
Co-authored-by: Adrian Dummermuth <[email protected]>
Co-authored-by: Julian Oes <[email protected]>

examples/gimbal_full_control/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.10.2)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+project(gimbal_full_control)
+
+add_executable(gimbal_full_control
+    gimbal_full_control.cpp
+)
+
+find_package(MAVSDK REQUIRED)
+
+target_link_libraries(gimbal_full_control
+    MAVSDK::mavsdk
+)
+
+if(NOT MSVC)
+    add_compile_options(gimbal PRIVATE -Wall -Wextra)
+else()
+    add_compile_options(gimbal PRIVATE -WX -W2)
+endif()

examples/gimbal_full_control/gimbal_full_control.cpp

@@ -0,0 +1,118 @@
+//
+// Example to demonstrate how to control a gimbal including roll.
+//
+// Can be tested against PX4 SITL with Typhoon H480:
+//     make px4_sitl gazebo_typhoon_h480
+
+#include <chrono>
+#include <cstdint>
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/gimbal/gimbal.h>
+#include <mavsdk/plugins/telemetry/telemetry.h>
+#include <iostream>
+#include <future>
+#include <memory>
+#include <thread>
+
+using namespace mavsdk;
+using std::chrono::seconds;
+using std::this_thread::sleep_for;
+
+void usage(const std::string& bin_name)
+{
+    std::cerr << "Usage : " << bin_name << " <connection_url>\n"
+              << "Connection URL format should be :\n"
+              << " For TCP : tcp://[server_host][:server_port]\n"
+              << " For UDP : udp://[bind_host][:bind_port]\n"
+              << " For Serial : serial:///path/to/serial/dev[:baudrate]\n"
+              << "For example, to connect to the simulator use URL: udp://:14540\n";
+}
+
+int main(int argc, char** argv)
+{
+    if (argc != 2) {
+        usage(argv[0]);
+        return 1;
+    }
+
+    Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}};
+    ConnectionResult connection_result = mavsdk.add_any_connection(argv[1]);
+
+    if (connection_result != ConnectionResult::Success) {
+        std::cerr << "Connection failed: " << connection_result << '\n';
+        return 1;
+    }
+
+    auto system = mavsdk.first_autopilot(3.0);
+    if (!system) {
+        std::cerr << "Timed out waiting for system\n";
+        return 1;
+    }
+
+    // Instantiate plugins.
+    auto telemetry = Telemetry{system.value()};
+    auto gimbal = Gimbal{system.value()};
+
+    // We want to listen to the camera/gimbal angle of the drone at 5 Hz.
+    const Telemetry::Result set_rate_result = telemetry.set_rate_camera_attitude(5.0);
+    if (set_rate_result != Telemetry::Result::Success) {
+        std::cerr << "Setting rate failed:" << set_rate_result << '\n';
+        return 1;
+    }
+
+    // Set up callback to monitor camera/gimbal angle
+    telemetry.subscribe_camera_attitude_euler([](Telemetry::EulerAngle angle) {
+        std::cout << "Gimbal angle pitch: " << angle.pitch_deg << " deg, yaw: " << angle.yaw_deg
+                  << " deg, roll: " << angle.roll_deg << " deg\n";
+    });
+
+    std::cout << "Start controlling gimbal...\n";
+    Gimbal::Result gimbal_result = gimbal.take_control(Gimbal::ControlMode::Primary);
+    if (gimbal_result != Gimbal::Result::Success) {
+        std::cerr << "Could not take gimbal control: " << gimbal_result << '\n';
+        return 1;
+    }
+
+    std::cout << "Set yaw mode to lock to a specific direction...\n";
+    gimbal_result = gimbal.set_mode(Gimbal::GimbalMode::YawLock);
+    if (gimbal_result != Gimbal::Result::Success) {
+        std::cerr << "Could not set to lock mode: " << gimbal_result << '\n';
+        return 1;
+    }
+
+    std::cout << "Test SetAngles...\n";
+    std::cout << "Roll=0 Pitch=0 Yaw=0\n";
+    gimbal.set_angles(0.0f, 0.0f, 0.0f);
+    sleep_for(seconds(5));
+    std::cout << "Roll=-30 Pitch=0 Yaw=0\n";
+    gimbal.set_angles(-30.0f, 0.0f, 0.0f);
+    sleep_for(seconds(5));
+    std::cout << "Roll=30 Pitch=0 Yaw=0\n";
+    gimbal.set_angles(30.0f, 0.0f, 0.0f);
+    sleep_for(seconds(5));
+    std::cout << "Roll=0 Pitch=-30 Yaw=0\n";
+    gimbal.set_angles(0.0f, -30.0f, 0.0f);
+    sleep_for(seconds(5));
+    std::cout << "Roll=0 Pitch=30 Yaw=0\n";
+    gimbal.set_angles(0.0f, 30.0f, 0.0f);
+    sleep_for(seconds(5));
+    std::cout << "Roll=0 Pitch=0 Yaw=-30\n";
+    gimbal.set_angles(0.0f, 0.0f, -30.0f);
+    sleep_for(seconds(5));
+    std::cout << "Roll=0 Pitch=0 Yaw=30\n";
+    gimbal.set_angles(0.0f, 0.0f, 30.0f);
+    sleep_for(seconds(5));
+    std::cout << "Roll=0 Pitch=0 Yaw=0\n";
+    gimbal.set_angles(0.0f, 0.0f, 0.0f);
+    sleep_for(seconds(2));
+
+    std::cout << "Stop controlling gimbal...\n";
+    gimbal_result = gimbal.release_control();
+    if (gimbal_result != Gimbal::Result::Success) {
+        std::cerr << "Could not take gimbal control: " << gimbal_result << '\n';
+        return 1;
+    }
+
+    std::cout << "Finished.\n";
+    return 0;
+}

src/mavsdk/core/CMakeLists.txt

@@ -121,7 +121,7 @@ if(ANDROID)
 endif()
 
 if((BUILD_STATIC_MAVSDK_SERVER AND ("${CMAKE_C_COMPILER_ID}" STREQUAL "GNU")) OR
-    (${CMAKE_HOST_SYSTEM_PROCESSOR} MATCHES "(armv6|armv7)"))
+    (${CMAKE_HOST_SYSTEM_PROCESSOR} MATCHES "(armv6|armv7|aarch64)"))
     target_link_libraries(mavsdk PRIVATE atomic)
 endif()
 

src/mavsdk/plugins/gimbal/gimbal.cpp

@@ -20,6 +20,17 @@ Gimbal::Gimbal(std::shared_ptr<System> system) :
 
 Gimbal::~Gimbal() {}
 
+void Gimbal::set_angles_async(
+    float roll_deg, float pitch_deg, float yaw_deg, const ResultCallback callback)
+{
+    _impl->set_angles_async(roll_deg, pitch_deg, yaw_deg, callback);
+}
+
+Gimbal::Result Gimbal::set_angles(float roll_deg, float pitch_deg, float yaw_deg) const
+{
+    return _impl->set_angles(roll_deg, pitch_deg, yaw_deg);
+}
+
 void Gimbal::set_pitch_and_yaw_async(float pitch_deg, float yaw_deg, const ResultCallback callback)
 {
     _impl->set_pitch_and_yaw_async(pitch_deg, yaw_deg, callback);

src/mavsdk/plugins/gimbal/gimbal_impl.cpp

@@ -79,6 +79,20 @@ void GimbalImpl::process_gimbal_manager_information(const mavlink_message_t& mes
     }
 }
 
+Gimbal::Result GimbalImpl::set_angles(float roll_deg, float pitch_deg, float yaw_deg)
+{
+    wait_for_protocol();
+
+    return _gimbal_protocol->set_angles(roll_deg, pitch_deg, yaw_deg);
+}
+
+void GimbalImpl::set_angles_async(
+    float roll_deg, float pitch_deg, float yaw_deg, Gimbal::ResultCallback callback)
+{
+    wait_for_protocol_async(
+        [=]() { _gimbal_protocol->set_angles_async(roll_deg, pitch_deg, yaw_deg, callback); });
+}
+
 Gimbal::Result GimbalImpl::set_pitch_and_yaw(float pitch_deg, float yaw_deg)
 {
     wait_for_protocol();

src/mavsdk/plugins/gimbal/gimbal_impl.h

@@ -20,6 +20,10 @@ class GimbalImpl : public PluginImplBase {
     void enable() override;
     void disable() override;
 
+    Gimbal::Result set_angles(float roll_deg, float pitch_deg, float yaw_deg);
+    void set_angles_async(
+        float roll_deg, float pitch_deg, float yaw_deg, Gimbal::ResultCallback callback);
+
     Gimbal::Result set_pitch_and_yaw(float pitch_deg, float yaw_deg);
     void set_pitch_and_yaw_async(float pitch_deg, float yaw_deg, Gimbal::ResultCallback callback);
 

src/mavsdk/plugins/gimbal/gimbal_protocol_base.h

@@ -11,6 +11,10 @@ class GimbalProtocolBase {
     GimbalProtocolBase(SystemImpl& system_impl) : _system_impl(system_impl) {}
     virtual ~GimbalProtocolBase() = default;
 
+    virtual Gimbal::Result set_angles(float roll_deg, float pitch_deg, float yaw_deg) = 0;
+    virtual void set_angles_async(
+        float roll_deg, float pitch_deg, float yaw_deg, Gimbal::ResultCallback callback) = 0;
+
     virtual Gimbal::Result set_pitch_and_yaw(float pitch_deg, float yaw_deg) = 0;
     virtual void
     set_pitch_and_yaw_async(float pitch_deg, float yaw_deg, Gimbal::ResultCallback callback) = 0;

src/mavsdk/plugins/gimbal/gimbal_protocol_v1.cpp

@@ -13,9 +13,8 @@ GimbalProtocolV1::~GimbalProtocolV1()
     _system_impl.remove_call_every(_control_cookie);
 }
 
-Gimbal::Result GimbalProtocolV1::set_pitch_and_yaw(float pitch_deg, float yaw_deg)
+Gimbal::Result GimbalProtocolV1::set_angles(float roll_deg, float pitch_deg, float yaw_deg)
 {
-    const float roll_deg = 0.0f;
     MavlinkCommandSender::CommandLong command{};
 
     command.command = MAV_CMD_DO_MOUNT_CONTROL;
@@ -28,10 +27,9 @@ Gimbal::Result GimbalProtocolV1::set_pitch_and_yaw(float pitch_deg, float yaw_de
     return GimbalImpl::gimbal_result_from_command_result(_system_impl.send_command(command));
 }
 
-void GimbalProtocolV1::set_pitch_and_yaw_async(
-    float pitch_deg, float yaw_deg, Gimbal::ResultCallback callback)
+void GimbalProtocolV1::set_angles_async(
+    float roll_deg, float pitch_deg, float yaw_deg, Gimbal::ResultCallback callback)
 {
-    const float roll_deg = 0.0f;
     MavlinkCommandSender::CommandLong command{};
 
     command.command = MAV_CMD_DO_MOUNT_CONTROL;
@@ -48,6 +46,17 @@ void GimbalProtocolV1::set_pitch_and_yaw_async(
         });
 }
 
+Gimbal::Result GimbalProtocolV1::set_pitch_and_yaw(float pitch_deg, float yaw_deg)
+{
+    return set_angles(0.0f, pitch_deg, yaw_deg);
+}
+
+void GimbalProtocolV1::set_pitch_and_yaw_async(
+    float pitch_deg, float yaw_deg, Gimbal::ResultCallback callback)
+{
+    set_angles_async(0.0f, pitch_deg, yaw_deg, callback);
+}
+
 Gimbal::Result
 GimbalProtocolV1::set_pitch_rate_and_yaw_rate(float pitch_rate_deg_s, float yaw_rate_deg_s)
 {

src/mavsdk/plugins/gimbal/gimbal_protocol_v1.h

@@ -13,6 +13,10 @@ class GimbalProtocolV1 : public GimbalProtocolBase {
     GimbalProtocolV1(SystemImpl& system_impl);
     ~GimbalProtocolV1() override;
 
+    Gimbal::Result set_angles(float roll_deg, float pitch_deg, float yaw_deg) override;
+    void set_angles_async(
+        float roll_deg, float pitch_deg, float yaw_deg, Gimbal::ResultCallback callback) override;
+
     Gimbal::Result set_pitch_and_yaw(float pitch_deg, float yaw_deg) override;
     void set_pitch_and_yaw_async(
         float pitch_deg, float yaw_deg, Gimbal::ResultCallback callback) override;

src/mavsdk/plugins/gimbal/gimbal_protocol_v2.cpp

@@ -61,9 +61,9 @@ void GimbalProtocolV2::process_gimbal_manager_status(const mavlink_message_t& me
     }
 }
 
-Gimbal::Result GimbalProtocolV2::set_pitch_and_yaw(float pitch_deg, float yaw_deg)
+Gimbal::Result GimbalProtocolV2::set_angles(float roll_deg, float pitch_deg, float yaw_deg)
 {
-    const float roll_rad = 0.0f;
+    const float roll_rad = to_rad_from_deg(roll_deg);
     const float pitch_rad = to_rad_from_deg(pitch_deg);
     const float yaw_rad = to_rad_from_deg(yaw_deg);
 
@@ -95,11 +95,27 @@ Gimbal::Result GimbalProtocolV2::set_pitch_and_yaw(float pitch_deg, float yaw_de
                Gimbal::Result::Error;
 }
 
+void GimbalProtocolV2::set_angles_async(
+    float roll_deg, float pitch_deg, float yaw_deg, Gimbal::ResultCallback callback)
+{
+    // Sending the message should be quick and we can just do that straighaway.
+    Gimbal::Result result = set_angles(roll_deg, pitch_deg, yaw_deg);
+
+    if (callback) {
+        _system_impl.call_user_callback([callback, result]() { callback(result); });
+    }
+}
+
+Gimbal::Result GimbalProtocolV2::set_pitch_and_yaw(float pitch_deg, float yaw_deg)
+{
+    return set_angles(0.0f, pitch_deg, yaw_deg);
+}
+
 void GimbalProtocolV2::set_pitch_and_yaw_async(
     float pitch_deg, float yaw_deg, Gimbal::ResultCallback callback)
 {
     // Sending the message should be quick and we can just do that straighaway.
-    Gimbal::Result result = set_pitch_and_yaw(pitch_deg, yaw_deg);
+    Gimbal::Result result = set_angles(0.0f, pitch_deg, yaw_deg);
 
     if (callback) {
         _system_impl.call_user_callback([callback, result]() { callback(result); });

src/mavsdk/plugins/gimbal/gimbal_protocol_v2.h

@@ -14,6 +14,10 @@ class GimbalProtocolV2 : public GimbalProtocolBase {
         uint8_t gimbal_manager_compid);
     ~GimbalProtocolV2() override;
 
+    Gimbal::Result set_angles(float roll_deg, float pitch_deg, float yaw_deg) override;
+    void set_angles_async(
+        float roll_deg, float pitch_deg, float yaw_deg, Gimbal::ResultCallback callback) override;
+
     Gimbal::Result set_pitch_and_yaw(float pitch_deg, float yaw_deg) override;
     void set_pitch_and_yaw_async(
         float pitch_deg, float yaw_deg, Gimbal::ResultCallback callback) override;

src/mavsdk/plugins/gimbal/include/plugins/gimbal/gimbal.h

@@ -143,6 +143,31 @@ class Gimbal : public PluginBase {
      */
     using ResultCallback = std::function<void(Result)>;
 
+    /**
+     * @brief Set gimbal roll, pitch and yaw angles.
+     *
+     * This sets the desired roll, pitch and yaw angles of a gimbal.
+     * Will return when the command is accepted, however, it might
+     * take the gimbal longer to actually be set to the new angles.
+     *
+     * This function is non-blocking. See 'set_angles' for the blocking counterpart.
+     */
+    void
+    set_angles_async(float roll_deg, float pitch_deg, float yaw_deg, const ResultCallback callback);
+
+    /**
+     * @brief Set gimbal roll, pitch and yaw angles.
+     *
+     * This sets the desired roll, pitch and yaw angles of a gimbal.
+     * Will return when the command is accepted, however, it might
+     * take the gimbal longer to actually be set to the new angles.
+     *
+     * This function is blocking. See 'set_angles_async' for the non-blocking counterpart.
+     *
+     * @return Result of request.
+     */
+    Result set_angles(float roll_deg, float pitch_deg, float yaw_deg) const;
+
     /**
      * @brief Set gimbal pitch and yaw angles.
      *