Skip to content

Commit

Permalink
Merge pull request #2298 from mavlink/pr-gimbal-fixes
Browse files Browse the repository at this point in the history 
gimbal: mutex fixes and added env variable for v2
  • Loading branch information
@julianoes
julianoes committed May 13, 2024
2 parents 68c5dc2 + d0b1e1c commit 6ff85c2
Show file tree
Hide file tree
Showing 2 changed files with 81 additions and 44 deletions.
120 changes: 78 additions & 42 deletions src/mavsdk/plugins/gimbal/gimbal_impl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,26 +44,39 @@ void GimbalImpl::deinit()

void GimbalImpl::enable()
{
_system_impl->register_timeout_handler(
[this]() { receive_protocol_timeout(); }, 1.0, &_protocol_cookie);
const char* env_p = std::getenv("MAVSDK_FORCE_GIMBAL_V2");
if (env_p && std::string(env_p) == "1") {
LogInfo() << "Forcing gimbal version 2";
} else {
if (_gimbal_protocol == nullptr) {
_system_impl->register_timeout_handler(
[this]() { receive_protocol_timeout(); }, 1.0, &_protocol_cookie);
}
}

MavlinkCommandSender::CommandLong command{};
command.command = MAV_CMD_REQUEST_MESSAGE;
command.params.maybe_param1 = {static_cast<float>(MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION)};
command.target_component_id = 0; // any component
_system_impl->send_command_async(command, nullptr);
request_gimbal_information();
}

void GimbalImpl::disable()
{
_system_impl->unregister_timeout_handler(_protocol_cookie);
}

void GimbalImpl::request_gimbal_information()
{
MavlinkCommandSender::CommandLong command{};
command.command = MAV_CMD_REQUEST_MESSAGE;
command.params.maybe_param1 = {static_cast<float>(MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION)};
command.target_component_id = 0; // any component
_system_impl->send_command_async(command, nullptr);
}

void GimbalImpl::receive_protocol_timeout()
{
// We did not receive a GIMBAL_MANAGER_INFORMATION in time, so we have to
// assume Version2 is not available.
LogWarn() << "Falling back to Gimbal Version 1";
std::lock_guard<std::mutex> lock(_mutex);
_gimbal_protocol.reset(new GimbalProtocolV1(*_system_impl));
_protocol_cookie = nullptr;
}
Expand All @@ -73,26 +86,26 @@ void GimbalImpl::process_gimbal_manager_information(const mavlink_message_t& mes
mavlink_gimbal_manager_information_t gimbal_manager_information;
mavlink_msg_gimbal_manager_information_decode(&message, &gimbal_manager_information);

if (_protocol_cookie != nullptr) {
LogDebug() << "Using Gimbal Version 2 as gimbal manager information for gimbal device "
<< static_cast<int>(gimbal_manager_information.gimbal_device_id)
<< " was discovered";

_system_impl->unregister_timeout_handler(_protocol_cookie);
_protocol_cookie = nullptr;

// We need to schedule the construction for later because it wants
// to register more message subscriptions which blocks.
// TODO: we should fix this at the callback list level
_system_impl->call_user_callback([=]() {
_gimbal_protocol.reset(new GimbalProtocolV2(
*_system_impl, gimbal_manager_information, message.sysid, message.compid));
_gimbal_protocol->attitude_async(
[this](auto attitude) { receive_attitude_update(attitude); });
_gimbal_protocol->control_async(
[this](auto control_status) { receive_control_status_update(control_status); });
});
}
_system_impl->unregister_timeout_handler(_protocol_cookie);

LogDebug() << "Using Gimbal Version 2 as gimbal manager information for gimbal device "
<< static_cast<int>(gimbal_manager_information.gimbal_device_id)
<< " was discovered";

_protocol_cookie = nullptr;

// We need to schedule the construction for later because it wants
// to register more message subscriptions which blocks.
// TODO: we should fix this at the callback list level
_system_impl->call_user_callback([=]() {
std::lock_guard<std::mutex> lock(_mutex);
_gimbal_protocol.reset(new GimbalProtocolV2(
*_system_impl, gimbal_manager_information, message.sysid, message.compid));
_gimbal_protocol->attitude_async(
[this](auto attitude) { receive_attitude_update(attitude); });
_gimbal_protocol->control_async(
[this](auto control_status) { receive_control_status_update(control_status); });
});
}

Gimbal::Result GimbalImpl::set_angles(float roll_deg, float pitch_deg, float yaw_deg)
Expand Down Expand Up @@ -198,26 +211,32 @@ Gimbal::ControlStatus GimbalImpl::control()

Gimbal::ControlHandle GimbalImpl::subscribe_control(const Gimbal::ControlCallback& callback)
{
std::lock_guard<std::mutex> lock(_mutex);
auto result = [&]() {
std::lock_guard<std::mutex> lock(_mutex);
bool need_to_register_callback = _control_subscriptions.empty();
auto handle = _control_subscriptions.subscribe(callback);
return std::pair<bool, Gimbal::ControlHandle>(need_to_register_callback, handle);
}();

bool need_to_register_callback = _control_subscriptions.empty();
auto handle = _control_subscriptions.subscribe(callback);

if (need_to_register_callback) {
if (result.first) {
wait_for_protocol_async([=]() {
// We don't lock here because we don't expect the protocol to change once it has been
// set.
_gimbal_protocol->control_async([this](Gimbal::ControlStatus status) {
_control_subscriptions.queue(
status, [this](const auto& func) { _system_impl->call_user_callback(func); });
});
});
}
return handle;
return result.second;
}

void GimbalImpl::unsubscribe_control(Gimbal::ControlHandle handle)
{
std::lock_guard<std::mutex> lock(_mutex);
_control_subscriptions.unsubscribe(handle);
{
std::lock_guard<std::mutex> lock(_mutex);
_control_subscriptions.unsubscribe(handle);
}

if (_control_subscriptions.empty()) {
wait_for_protocol_async([=]() { _gimbal_protocol->control_async(nullptr); });
Expand All @@ -226,20 +245,24 @@ void GimbalImpl::unsubscribe_control(Gimbal::ControlHandle handle)

Gimbal::AttitudeHandle GimbalImpl::subscribe_attitude(const Gimbal::AttitudeCallback& callback)
{
std::lock_guard<std::mutex> lock(_mutex);

bool need_to_register_callback = _attitude_subscriptions.empty();
auto handle = _attitude_subscriptions.subscribe(callback);
auto result = [&]() {
// We don't lock here because we don't expect the protocol to change once it has been set.
bool need_to_register_callback = _attitude_subscriptions.empty();
auto handle = _attitude_subscriptions.subscribe(callback);
return std::pair<bool, Gimbal::AttitudeHandle>(need_to_register_callback, handle);
}();

if (need_to_register_callback) {
if (result.first) {
wait_for_protocol_async([=]() {
// We don't lock here because we don't expect the protocol to change once it has been
// set.
_gimbal_protocol->attitude_async([this](Gimbal::Attitude attitude) {
_attitude_subscriptions.queue(
attitude, [this](const auto& func) { _system_impl->call_user_callback(func); });
});
});
}
return handle;
return result.second;
}

void GimbalImpl::unsubscribe_attitude(Gimbal::AttitudeHandle handle)
Expand All @@ -255,13 +278,26 @@ void GimbalImpl::unsubscribe_attitude(Gimbal::AttitudeHandle handle)
Gimbal::Attitude GimbalImpl::attitude()
{
wait_for_protocol();
// We don't lock here because we don't expect the protocol to change once it has been set.
return _gimbal_protocol->attitude();
}

void GimbalImpl::wait_for_protocol()
{
while (_gimbal_protocol == nullptr) {
unsigned counter = 0;
while (true) {
{
std::lock_guard<std::mutex> lock(_mutex);
if (_gimbal_protocol != nullptr) {
break;
}
}
// Request gimbal information every 3 seconds again
if (counter % 30 == 0) {
request_gimbal_information();
}
std::this_thread::sleep_for(std::chrono::milliseconds(100));
++counter;
}
}

Expand Down
5 changes: 3 additions & 2 deletions src/mavsdk/plugins/gimbal/gimbal_impl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,11 +67,11 @@ class GimbalImpl : public PluginImplBase {
const GimbalImpl& operator=(const GimbalImpl&) = delete;

private:
void request_gimbal_information();

void receive_attitude_update(Gimbal::Attitude attitude);
void receive_control_status_update(Gimbal::ControlStatus control_status);

std::unique_ptr<GimbalProtocolBase> _gimbal_protocol{nullptr};

void* _protocol_cookie{nullptr};

void wait_for_protocol();
Expand All @@ -80,6 +80,7 @@ class GimbalImpl : public PluginImplBase {
void process_gimbal_manager_information(const mavlink_message_t& message);

std::mutex _mutex{};
std::unique_ptr<GimbalProtocolBase> _gimbal_protocol{nullptr};
CallbackList<Gimbal::ControlStatus> _control_subscriptions{};
CallbackList<Gimbal::Attitude> _attitude_subscriptions{};
};
Expand Down

0 comments on commit 6ff85c2

Please sign in to comment.