Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Full gimbal angle control #2210

Merged
merged 14 commits into from
Jan 23, 2024
22 changes: 22 additions & 0 deletions examples/gimbal_full_control/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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()
118 changes: 118 additions & 0 deletions examples/gimbal_full_control/gimbal_full_control.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,118 @@
//
// Simple example to demonstrate how to control a gimbal.
//
// 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;
}
2 changes: 1 addition & 1 deletion proto
2 changes: 1 addition & 1 deletion src/mavsdk/core/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)"))
julianoes marked this conversation as resolved.
Show resolved Hide resolved
target_link_libraries(mavsdk PRIVATE atomic)
endif()

Expand Down
11 changes: 11 additions & 0 deletions src/mavsdk/plugins/gimbal/gimbal.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
14 changes: 14 additions & 0 deletions src/mavsdk/plugins/gimbal/gimbal_impl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
4 changes: 4 additions & 0 deletions src/mavsdk/plugins/gimbal/gimbal_impl.h
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
4 changes: 4 additions & 0 deletions src/mavsdk/plugins/gimbal/gimbal_protocol_base.h
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
19 changes: 14 additions & 5 deletions src/mavsdk/plugins/gimbal/gimbal_protocol_v1.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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)
{
Expand Down
4 changes: 4 additions & 0 deletions src/mavsdk/plugins/gimbal/gimbal_protocol_v1.h
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
22 changes: 19 additions & 3 deletions src/mavsdk/plugins/gimbal/gimbal_protocol_v2.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -52,9 +52,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);

Expand Down Expand Up @@ -86,11 +86,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); });
Expand Down
4 changes: 4 additions & 0 deletions src/mavsdk/plugins/gimbal/gimbal_protocol_v2.h
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,10 @@ class GimbalProtocolV2 : public GimbalProtocolBase {
uint8_t gimbal_manager_compid);
~GimbalProtocolV2() override = default;

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;
Expand Down
25 changes: 25 additions & 0 deletions src/mavsdk/plugins/gimbal/include/plugins/gimbal/gimbal.h
Original file line number Diff line number Diff line change
Expand Up @@ -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.
*
Expand Down
Loading
Loading