diff --git a/src/mavsdk/plugins/gimbal/gimbal_impl.cpp b/src/mavsdk/plugins/gimbal/gimbal_impl.cpp index 5c83eb949..484b35d89 100644 --- a/src/mavsdk/plugins/gimbal/gimbal_impl.cpp +++ b/src/mavsdk/plugins/gimbal/gimbal_impl.cpp @@ -44,14 +44,17 @@ 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(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() @@ -59,11 +62,21 @@ 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(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 lock(_mutex); _gimbal_protocol.reset(new GimbalProtocolV1(*_system_impl)); _protocol_cookie = nullptr; } @@ -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(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(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 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) @@ -198,26 +211,32 @@ Gimbal::ControlStatus GimbalImpl::control() Gimbal::ControlHandle GimbalImpl::subscribe_control(const Gimbal::ControlCallback& callback) { - std::lock_guard lock(_mutex); + auto result = [&]() { + std::lock_guard lock(_mutex); + bool need_to_register_callback = _control_subscriptions.empty(); + auto handle = _control_subscriptions.subscribe(callback); + return std::pair(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 lock(_mutex); - _control_subscriptions.unsubscribe(handle); + { + std::lock_guard lock(_mutex); + _control_subscriptions.unsubscribe(handle); + } if (_control_subscriptions.empty()) { wait_for_protocol_async([=]() { _gimbal_protocol->control_async(nullptr); }); @@ -226,20 +245,24 @@ void GimbalImpl::unsubscribe_control(Gimbal::ControlHandle handle) Gimbal::AttitudeHandle GimbalImpl::subscribe_attitude(const Gimbal::AttitudeCallback& callback) { - std::lock_guard 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(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) @@ -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 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; } } diff --git a/src/mavsdk/plugins/gimbal/gimbal_impl.h b/src/mavsdk/plugins/gimbal/gimbal_impl.h index 7dde8aaa8..00cd99764 100644 --- a/src/mavsdk/plugins/gimbal/gimbal_impl.h +++ b/src/mavsdk/plugins/gimbal/gimbal_impl.h @@ -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 _gimbal_protocol{nullptr}; - void* _protocol_cookie{nullptr}; void wait_for_protocol(); @@ -80,6 +80,7 @@ class GimbalImpl : public PluginImplBase { void process_gimbal_manager_information(const mavlink_message_t& message); std::mutex _mutex{}; + std::unique_ptr _gimbal_protocol{nullptr}; CallbackList _control_subscriptions{}; CallbackList _attitude_subscriptions{}; };