From 03a803461d887113fe85ffeb991aa5631c72e2c6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 26 Jul 2024 01:48:32 +1200 Subject: [PATCH] gimbal: remove old protocol It's time to sunset the old gimbal v1 protocol and simplify the gimbal plugin implementation around v2. The auto-detection between v1 and v2 had quite a few problems, so it was hurting us. --- src/mavsdk/plugins/gimbal/CMakeLists.txt | 2 - src/mavsdk/plugins/gimbal/gimbal_impl.cpp | 514 ++++++++++++------ src/mavsdk/plugins/gimbal/gimbal_impl.h | 22 +- .../plugins/gimbal/gimbal_protocol_base.h | 57 -- .../plugins/gimbal/gimbal_protocol_v1.cpp | 250 --------- .../plugins/gimbal/gimbal_protocol_v1.h | 67 --- .../plugins/gimbal/gimbal_protocol_v2.cpp | 469 ---------------- .../plugins/gimbal/gimbal_protocol_v2.h | 76 --- 8 files changed, 359 insertions(+), 1098 deletions(-) delete mode 100644 src/mavsdk/plugins/gimbal/gimbal_protocol_base.h delete mode 100644 src/mavsdk/plugins/gimbal/gimbal_protocol_v1.cpp delete mode 100644 src/mavsdk/plugins/gimbal/gimbal_protocol_v1.h delete mode 100644 src/mavsdk/plugins/gimbal/gimbal_protocol_v2.cpp delete mode 100644 src/mavsdk/plugins/gimbal/gimbal_protocol_v2.h diff --git a/src/mavsdk/plugins/gimbal/CMakeLists.txt b/src/mavsdk/plugins/gimbal/CMakeLists.txt index af37cae51b..990ef63fc5 100644 --- a/src/mavsdk/plugins/gimbal/CMakeLists.txt +++ b/src/mavsdk/plugins/gimbal/CMakeLists.txt @@ -2,8 +2,6 @@ target_sources(mavsdk PRIVATE gimbal.cpp gimbal_impl.cpp - gimbal_protocol_v1.cpp - gimbal_protocol_v2.cpp ) target_include_directories(mavsdk PUBLIC diff --git a/src/mavsdk/plugins/gimbal/gimbal_impl.cpp b/src/mavsdk/plugins/gimbal/gimbal_impl.cpp index f59fb44740..4998808bf5 100644 --- a/src/mavsdk/plugins/gimbal/gimbal_impl.cpp +++ b/src/mavsdk/plugins/gimbal/gimbal_impl.cpp @@ -1,7 +1,7 @@ #include "gimbal_impl.h" -#include "gimbal_protocol_v1.h" -#include "gimbal_protocol_v2.h" #include "callback_list.tpp" +#include "mavsdk_math.h" +#include "math_conversions.h" #include #include #include @@ -32,35 +32,36 @@ void GimbalImpl::init() MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION, [this](const mavlink_message_t& message) { process_gimbal_manager_information(message); }, this); + + _system_impl->register_mavlink_message_handler( + MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS, + [this](const mavlink_message_t& message) { process_gimbal_manager_status(message); }, + this); + + _system_impl->register_mavlink_message_handler( + MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, + [this](const mavlink_message_t& message) { + process_gimbal_device_attitude_status(message); + }, + this); + + _system_impl->register_mavlink_message_handler( + MAVLINK_MSG_ID_ATTITUDE, + [this](const mavlink_message_t& message) { process_attitude(message); }, + this); } void GimbalImpl::deinit() { - std::lock_guard lock(_mutex); - - _gimbal_protocol.reset(nullptr); _system_impl->unregister_all_mavlink_message_handlers(this); } void GimbalImpl::enable() { - 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) { - _protocol_cookie = _system_impl->register_timeout_handler( - [this]() { receive_protocol_timeout(); }, 1.0); - } - } - request_gimbal_information(); } -void GimbalImpl::disable() -{ - _system_impl->unregister_timeout_handler(_protocol_cookie); -} +void GimbalImpl::disable() {} void GimbalImpl::request_gimbal_information() { @@ -71,238 +72,433 @@ void GimbalImpl::request_gimbal_information() _system_impl->send_command_async(command, nullptr); } -void GimbalImpl::receive_protocol_timeout() +void GimbalImpl::process_gimbal_manager_information(const mavlink_message_t& message) { - // 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"; + mavlink_gimbal_manager_information_t gimbal_manager_information; + mavlink_msg_gimbal_manager_information_decode(&message, &gimbal_manager_information); + + // TODO: implement +} + +void GimbalImpl::process_gimbal_manager_status(const mavlink_message_t& message) +{ + mavlink_gimbal_manager_status_t status; + mavlink_msg_gimbal_manager_status_decode(&message, &status); + std::lock_guard lock(_mutex); - _gimbal_protocol.reset(new GimbalProtocolV1(*_system_impl)); - _protocol_cookie = {}; + + if (status.primary_control_sysid == static_cast(_system_impl->get_own_system_id()) && + status.primary_control_compid == static_cast(_system_impl->get_own_component_id())) { + _control_status.control_mode = Gimbal::ControlMode::Primary; + } else if ( + status.secondary_control_sysid == static_cast(_system_impl->get_own_system_id()) && + status.secondary_control_compid == static_cast(_system_impl->get_own_component_id())) { + _control_status.control_mode = Gimbal::ControlMode::Secondary; + } else { + _control_status.control_mode = Gimbal::ControlMode::None; + } + + _control_status.sysid_primary_control = status.primary_control_sysid; + _control_status.compid_primary_control = status.primary_control_compid; + _control_status.sysid_secondary_control = status.secondary_control_sysid; + _control_status.compid_secondary_control = status.secondary_control_compid; + + _control_subscriptions.queue( + _control_status, [this](const auto& func) { _system_impl->call_user_callback(func); }); } -void GimbalImpl::process_gimbal_manager_information(const mavlink_message_t& message) +void GimbalImpl::process_gimbal_device_attitude_status(const mavlink_message_t& message) { - mavlink_gimbal_manager_information_t gimbal_manager_information; - mavlink_msg_gimbal_manager_information_decode(&message, &gimbal_manager_information); + mavlink_gimbal_device_attitude_status_t attitude_status; + mavlink_msg_gimbal_device_attitude_status_decode(&message, &attitude_status); - _system_impl->unregister_timeout_handler(_protocol_cookie); + // By default, we assume it's in vehicle/forward frame. + bool is_in_forward_frame = true; - LogDebug() << "Using Gimbal Version 2 as gimbal manager information for gimbal device " - << static_cast(gimbal_manager_information.gimbal_device_id) - << " was discovered"; + if (attitude_status.flags & GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME || + attitude_status.flags & GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME) { + // Flags are set correctly according to newer spec, so we can use it. + if (attitude_status.flags & GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME) { + is_in_forward_frame = false; + } + } else { + // Neither of the flags indicating the frame are set, we fallback to previous way + // which depends on lock flag. + if (attitude_status.flags & GIMBAL_DEVICE_FLAGS_YAW_LOCK) { + is_in_forward_frame = false; + } + } - _protocol_cookie = {}; + std::lock_guard lock(_mutex); - // 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); }); - }); + // Reset to defaults (e.g. NaN) first. + _attitude = {}; + + if (is_in_forward_frame) { + _attitude.quaternion_forward.w = attitude_status.q[0]; + _attitude.quaternion_forward.x = attitude_status.q[1]; + _attitude.quaternion_forward.y = attitude_status.q[2]; + _attitude.quaternion_forward.z = attitude_status.q[3]; + + auto quaternion_forward = Quaternion{}; + quaternion_forward.w = attitude_status.q[0]; + quaternion_forward.x = attitude_status.q[1]; + quaternion_forward.y = attitude_status.q[2]; + quaternion_forward.z = attitude_status.q[3]; + const auto euler_angle_forward = to_euler_angle_from_quaternion(quaternion_forward); + + _attitude.euler_angle_forward.roll_deg = euler_angle_forward.roll_deg; + _attitude.euler_angle_forward.pitch_deg = euler_angle_forward.pitch_deg; + _attitude.euler_angle_forward.yaw_deg = euler_angle_forward.yaw_deg; + + _attitude.timestamp_us = attitude_status.time_boot_ms * 1000; + + // Calculate angle relative to North as well + if (!std::isnan(_vehicle_yaw_rad)) { + auto rotation = + to_quaternion_from_euler_angle(EulerAngle{0, 0, to_deg_from_rad(_vehicle_yaw_rad)}); + auto quaternion_north = rotation * quaternion_forward; + + _attitude.quaternion_north.w = quaternion_north.w; + _attitude.quaternion_north.x = quaternion_north.x; + _attitude.quaternion_north.y = quaternion_north.y; + _attitude.quaternion_north.z = quaternion_north.z; + + const auto euler_angle_north = to_euler_angle_from_quaternion(quaternion_north); + _attitude.euler_angle_north.roll_deg = euler_angle_north.roll_deg; + _attitude.euler_angle_north.pitch_deg = euler_angle_north.pitch_deg; + _attitude.euler_angle_north.yaw_deg = euler_angle_north.yaw_deg; + } + + } else { + _attitude.quaternion_north.w = attitude_status.q[0]; + _attitude.quaternion_north.x = attitude_status.q[1]; + _attitude.quaternion_north.y = attitude_status.q[2]; + _attitude.quaternion_north.z = attitude_status.q[3]; + + auto quaternion_north = Quaternion{}; + quaternion_north.w = attitude_status.q[0]; + quaternion_north.x = attitude_status.q[1]; + quaternion_north.y = attitude_status.q[2]; + quaternion_north.z = attitude_status.q[3]; + const auto euler_angle_north = to_euler_angle_from_quaternion(quaternion_north); + + _attitude.euler_angle_north.roll_deg = euler_angle_north.roll_deg; + _attitude.euler_angle_north.pitch_deg = euler_angle_north.pitch_deg; + _attitude.euler_angle_north.yaw_deg = euler_angle_north.yaw_deg; + + // Calculate angle relative to forward as well + if (!std::isnan(_vehicle_yaw_rad)) { + auto rotation = to_quaternion_from_euler_angle( + EulerAngle{0, 0, -to_deg_from_rad(_vehicle_yaw_rad)}); + auto quaternion_forward = rotation * quaternion_north; + + _attitude.quaternion_forward.w = quaternion_forward.w; + _attitude.quaternion_forward.x = quaternion_forward.x; + _attitude.quaternion_forward.y = quaternion_forward.y; + _attitude.quaternion_forward.z = quaternion_forward.z; + + const auto euler_angle_forward = to_euler_angle_from_quaternion(quaternion_forward); + _attitude.euler_angle_forward.roll_deg = euler_angle_forward.roll_deg; + _attitude.euler_angle_forward.pitch_deg = euler_angle_forward.pitch_deg; + _attitude.euler_angle_forward.yaw_deg = euler_angle_forward.yaw_deg; + } + } + + _attitude.angular_velocity.roll_rad_s = attitude_status.angular_velocity_x; + _attitude.angular_velocity.pitch_rad_s = attitude_status.angular_velocity_y; + _attitude.angular_velocity.yaw_rad_s = attitude_status.angular_velocity_z; + + _attitude_subscriptions.queue( + _attitude, [this](const auto& func) { _system_impl->call_user_callback(func); }); +} + +void GimbalImpl::process_attitude(const mavlink_message_t& message) +{ + mavlink_attitude_t attitude; + mavlink_msg_attitude_decode(&message, &attitude); + + std::lock_guard lock(_mutex); + + _vehicle_yaw_rad = attitude.yaw; } Gimbal::Result GimbalImpl::set_angles(float roll_deg, float pitch_deg, float yaw_deg) { - wait_for_protocol(); + 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); + + float quaternion[4]; + mavlink_euler_to_quaternion(roll_rad, pitch_rad, yaw_rad, quaternion); + + std::lock_guard lock(_mutex); - return _gimbal_protocol->set_angles(roll_deg, pitch_deg, yaw_deg); + const uint32_t flags = + GIMBAL_MANAGER_FLAGS_ROLL_LOCK | GIMBAL_MANAGER_FLAGS_PITCH_LOCK | + ((_gimbal_mode == Gimbal::GimbalMode::YawLock) ? GIMBAL_MANAGER_FLAGS_YAW_LOCK : 0); + + return _system_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { + mavlink_message_t message; + mavlink_msg_gimbal_manager_set_attitude_pack_chan( + mavlink_address.system_id, + mavlink_address.component_id, + channel, + &message, + _gimbal_manager_sysid, + _gimbal_manager_compid, + flags, + _gimbal_device_id, + quaternion, + NAN, + NAN, + NAN); + return message; + }) ? + Gimbal::Result::Success : + Gimbal::Result::Error; } 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); }); + // Sending the message should be quick and we can just do that straighaway. + Gimbal::Result result = set_angles(roll_deg, pitch_deg, yaw_deg); + + std::lock_guard lock(_mutex); + + if (callback) { + _system_impl->call_user_callback([callback, result]() { callback(result); }); + } } Gimbal::Result GimbalImpl::set_pitch_and_yaw(float pitch_deg, float yaw_deg) { - wait_for_protocol(); - - return _gimbal_protocol->set_pitch_and_yaw(pitch_deg, yaw_deg); + return set_angles(0.0f, pitch_deg, yaw_deg); } void GimbalImpl::set_pitch_and_yaw_async( float pitch_deg, float yaw_deg, Gimbal::ResultCallback callback) { - wait_for_protocol_async( - [=]() { _gimbal_protocol->set_pitch_and_yaw_async(pitch_deg, yaw_deg, callback); }); + // Sending the message should be quick and we can just do that straighaway. + set_angles_async(0.0f, pitch_deg, yaw_deg, callback); } Gimbal::Result GimbalImpl::set_pitch_rate_and_yaw_rate(float pitch_rate_deg_s, float yaw_rate_deg_s) { - wait_for_protocol(); + std::lock_guard lock(_mutex); - return _gimbal_protocol->set_pitch_rate_and_yaw_rate(pitch_rate_deg_s, yaw_rate_deg_s); + const uint32_t flags = + GIMBAL_MANAGER_FLAGS_ROLL_LOCK | GIMBAL_MANAGER_FLAGS_PITCH_LOCK | + ((_gimbal_mode == Gimbal::GimbalMode::YawLock) ? GIMBAL_MANAGER_FLAGS_YAW_LOCK : 0); + + const float quaternion[4] = {NAN, NAN, NAN, NAN}; + + return _system_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { + mavlink_message_t message; + mavlink_msg_gimbal_manager_set_attitude_pack_chan( + mavlink_address.system_id, + mavlink_address.component_id, + channel, + &message, + _gimbal_manager_sysid, + _gimbal_manager_compid, + flags, + _gimbal_device_id, + quaternion, + 0.0f, + to_rad_from_deg(pitch_rate_deg_s), + to_rad_from_deg(yaw_rate_deg_s)); + return message; + }) ? + Gimbal::Result::Success : + Gimbal::Result::Error; } void GimbalImpl::set_pitch_rate_and_yaw_rate_async( float pitch_rate_deg_s, float yaw_rate_deg_s, Gimbal::ResultCallback callback) { - wait_for_protocol_async([=]() { - _gimbal_protocol->set_pitch_rate_and_yaw_rate_async( - pitch_rate_deg_s, yaw_rate_deg_s, callback); - }); + // Sending the message should be quick and we can just do that straighaway. + Gimbal::Result result = set_pitch_rate_and_yaw_rate(pitch_rate_deg_s, yaw_rate_deg_s); + + std::lock_guard lock(_mutex); + + if (callback) { + auto temp_callback = callback; + _system_impl->call_user_callback([temp_callback, result]() { temp_callback(result); }); + } } Gimbal::Result GimbalImpl::set_mode(const Gimbal::GimbalMode gimbal_mode) { - wait_for_protocol(); - return _gimbal_protocol->set_mode(gimbal_mode); + std::lock_guard lock(_mutex); + + _gimbal_mode = gimbal_mode; + return Gimbal::Result::Success; } void GimbalImpl::set_mode_async( const Gimbal::GimbalMode gimbal_mode, Gimbal::ResultCallback callback) { - wait_for_protocol_async([=]() { _gimbal_protocol->set_mode_async(gimbal_mode, callback); }); + std::lock_guard lock(_mutex); + + _gimbal_mode = gimbal_mode; + + if (callback) { + _system_impl->call_user_callback([callback]() { callback(Gimbal::Result::Success); }); + } } Gimbal::Result GimbalImpl::set_roi_location(double latitude_deg, double longitude_deg, float altitude_m) { - wait_for_protocol(); - return _gimbal_protocol->set_roi_location(latitude_deg, longitude_deg, altitude_m); + MavlinkCommandSender::CommandInt command{}; + + command.command = MAV_CMD_DO_SET_ROI_LOCATION; + command.params.x = static_cast(std::round(latitude_deg * 1e7)); + command.params.y = static_cast(std::round(longitude_deg * 1e7)); + command.params.maybe_z = altitude_m; + command.target_system_id = _gimbal_manager_sysid; + command.target_component_id = _gimbal_manager_compid; + + return gimbal_result_from_command_result(_system_impl->send_command(command)); } void GimbalImpl::set_roi_location_async( double latitude_deg, double longitude_deg, float altitude_m, Gimbal::ResultCallback callback) { - wait_for_protocol_async([=]() { - _gimbal_protocol->set_roi_location_async(latitude_deg, longitude_deg, altitude_m, callback); - }); + std::lock_guard lock(_mutex); + + MavlinkCommandSender::CommandInt command{}; + + command.command = MAV_CMD_DO_SET_ROI_LOCATION; + command.params.x = static_cast(std::round(latitude_deg * 1e7)); + command.params.y = static_cast(std::round(longitude_deg * 1e7)); + command.params.maybe_z = altitude_m; + command.target_system_id = _gimbal_manager_sysid; + command.target_component_id = _gimbal_manager_compid; + + _system_impl->send_command_async( + command, [callback](MavlinkCommandSender::Result result, float) { + receive_command_result(result, callback); + }); } Gimbal::Result GimbalImpl::take_control(Gimbal::ControlMode control_mode) { - wait_for_protocol(); - return _gimbal_protocol->take_control(control_mode); + auto prom = std::promise(); + auto fut = prom.get_future(); + + take_control_async(control_mode, [&prom](Gimbal::Result result) { prom.set_value(result); }); + + return fut.get(); } void GimbalImpl::take_control_async( Gimbal::ControlMode control_mode, Gimbal::ResultCallback callback) { - wait_for_protocol_async( - [=]() { _gimbal_protocol->take_control_async(control_mode, callback); }); + std::lock_guard lock(_mutex); + + if (control_mode == Gimbal::ControlMode::None) { + release_control_async(callback); + return; + } else if (control_mode == Gimbal::ControlMode::Secondary) { + _system_impl->call_user_callback([callback]() { callback(Gimbal::Result::Unsupported); }); + LogErr() << "Gimbal secondary control is not implemented yet!"; + return; + } + + float own_sysid = _system_impl->get_own_system_id(); + float own_compid = _system_impl->get_own_component_id(); + + MavlinkCommandSender::CommandLong command{}; + + command.command = MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + command.params.maybe_param1 = + control_mode == Gimbal::ControlMode::Primary ? own_sysid : -3.0f; // sysid primary control + command.params.maybe_param2 = + control_mode == Gimbal::ControlMode::Primary ? own_compid : -3.0f; // compid primary control + command.params.maybe_param3 = + control_mode == Gimbal::ControlMode::Primary ? own_sysid : -3.0f; // sysid secondary control + command.params.maybe_param4 = control_mode == Gimbal::ControlMode::Primary ? + own_compid : + -3.0f; // compid secondary control + + command.params.maybe_param7 = _gimbal_device_id; + command.target_system_id = _gimbal_manager_sysid; + command.target_component_id = _gimbal_manager_compid; + + _system_impl->send_command_async( + command, [callback](MavlinkCommandSender::Result result, float) { + GimbalImpl::receive_command_result(result, callback); + }); } Gimbal::Result GimbalImpl::release_control() { - wait_for_protocol(); - return _gimbal_protocol->release_control(); + auto prom = std::promise(); + auto fut = prom.get_future(); + + release_control_async([&prom](Gimbal::Result result) { prom.set_value(result); }); + + return fut.get(); } void GimbalImpl::release_control_async(Gimbal::ResultCallback callback) { - wait_for_protocol_async([=]() { _gimbal_protocol->release_control_async(callback); }); + std::lock_guard lock(_mutex); + + MavlinkCommandSender::CommandLong command{}; + + command.command = MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + command.params.maybe_param1 = -3.0f; // sysid primary control + command.params.maybe_param2 = -3.0f; // compid primary control + command.params.maybe_param3 = -3.0f; // sysid secondary control + command.params.maybe_param4 = -3.0f; // compid secondary control + command.params.maybe_param7 = _gimbal_device_id; + command.target_system_id = _gimbal_manager_sysid; + command.target_component_id = _gimbal_manager_compid; + + _system_impl->send_command_async( + command, [callback](MavlinkCommandSender::Result result, float) { + receive_command_result(result, callback); + }); } Gimbal::ControlStatus GimbalImpl::control() { - wait_for_protocol(); - return _gimbal_protocol->control(); + std::lock_guard lock(_mutex); + return _control_status; } Gimbal::ControlHandle GimbalImpl::subscribe_control(const Gimbal::ControlCallback& callback) { - 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); - }(); - - if (result.first) { - wait_for_protocol_async([=]() { - std::lock_guard lock(_mutex); - _gimbal_protocol->control_async([this](Gimbal::ControlStatus status) { - _control_subscriptions.queue( - status, [this](const auto& func) { _system_impl->call_user_callback(func); }); - }); - }); - } - return result.second; + std::lock_guard lock(_mutex); + return _control_subscriptions.subscribe(callback); } void GimbalImpl::unsubscribe_control(Gimbal::ControlHandle handle) { - { - std::lock_guard lock(_mutex); - _control_subscriptions.unsubscribe(handle); - } - - if (_control_subscriptions.empty()) { - wait_for_protocol_async([=]() { _gimbal_protocol->control_async(nullptr); }); - } + std::lock_guard lock(_mutex); + _control_subscriptions.unsubscribe(handle); } Gimbal::AttitudeHandle GimbalImpl::subscribe_attitude(const Gimbal::AttitudeCallback& 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 (result.first) { - wait_for_protocol_async([=]() { - std::lock_guard lock(_mutex); - _gimbal_protocol->attitude_async([this](Gimbal::Attitude attitude) { - _attitude_subscriptions.queue( - attitude, [this](const auto& func) { _system_impl->call_user_callback(func); }); - }); - }); - } - return result.second; + std::lock_guard lock(_mutex); + return _attitude_subscriptions.subscribe(callback); } void GimbalImpl::unsubscribe_attitude(Gimbal::AttitudeHandle handle) { std::lock_guard lock(_mutex); _attitude_subscriptions.unsubscribe(handle); - - if (_attitude_subscriptions.empty()) { - wait_for_protocol_async([=]() { _gimbal_protocol->attitude_async(nullptr); }); - } } 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() -{ - 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; - } -} - -void GimbalImpl::wait_for_protocol_async(std::function callback) -{ - wait_for_protocol(); - callback(); + std::lock_guard lock(_mutex); + return _attitude; } void GimbalImpl::receive_command_result( @@ -315,22 +511,6 @@ void GimbalImpl::receive_command_result( } } -void GimbalImpl::receive_attitude_update(Gimbal::Attitude attitude) -{ - std::lock_guard lock(_mutex); - - _attitude_subscriptions.queue( - attitude, [this](const auto& func) { _system_impl->call_user_callback(func); }); -} - -void GimbalImpl::receive_control_status_update(Gimbal::ControlStatus control_status) -{ - std::lock_guard lock(_mutex); - - _control_subscriptions.queue( - control_status, [this](const auto& func) { _system_impl->call_user_callback(func); }); -} - Gimbal::Result GimbalImpl::gimbal_result_from_command_result(MavlinkCommandSender::Result command_result) { diff --git a/src/mavsdk/plugins/gimbal/gimbal_impl.h b/src/mavsdk/plugins/gimbal/gimbal_impl.h index 035c0ea61c..f6606c2278 100644 --- a/src/mavsdk/plugins/gimbal/gimbal_impl.h +++ b/src/mavsdk/plugins/gimbal/gimbal_impl.h @@ -1,7 +1,6 @@ #pragma once #include "plugins/gimbal/gimbal.h" -#include "gimbal_protocol_base.h" #include "plugin_impl_base.h" #include "system.h" #include "callback_list.h" @@ -69,20 +68,23 @@ class GimbalImpl : public PluginImplBase { private: void request_gimbal_information(); - void receive_attitude_update(Gimbal::Attitude attitude); - void receive_control_status_update(Gimbal::ControlStatus control_status); - - TimeoutHandler::Cookie _protocol_cookie{}; - - void wait_for_protocol(); - void wait_for_protocol_async(std::function callback); - void receive_protocol_timeout(); void process_gimbal_manager_information(const mavlink_message_t& message); + void process_gimbal_manager_status(const mavlink_message_t& message); + void process_gimbal_device_attitude_status(const mavlink_message_t& message); + void process_attitude(const mavlink_message_t& message); std::mutex _mutex{}; - std::unique_ptr _gimbal_protocol{nullptr}; CallbackList _control_subscriptions{}; CallbackList _attitude_subscriptions{}; + + uint8_t _gimbal_device_id{0}; + uint8_t _gimbal_manager_sysid{0}; + uint8_t _gimbal_manager_compid{0}; + + Gimbal::GimbalMode _gimbal_mode{Gimbal::GimbalMode::YawFollow}; + Gimbal::ControlStatus _control_status{Gimbal::ControlMode::None, 0, 0, 0, 0}; + Gimbal::Attitude _attitude{}; + float _vehicle_yaw_rad{NAN}; }; } // namespace mavsdk diff --git a/src/mavsdk/plugins/gimbal/gimbal_protocol_base.h b/src/mavsdk/plugins/gimbal/gimbal_protocol_base.h deleted file mode 100644 index 6f940789e2..0000000000 --- a/src/mavsdk/plugins/gimbal/gimbal_protocol_base.h +++ /dev/null @@ -1,57 +0,0 @@ -#pragma once - -#include "plugins/gimbal/gimbal.h" -#include "plugin_impl_base.h" -#include "system_impl.h" - -namespace mavsdk { - -class GimbalProtocolBase { -public: - 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; - - virtual Gimbal::Result - set_pitch_rate_and_yaw_rate(float pitch_rate_deg_s, float yaw_rate_deg_s) = 0; - - virtual void set_pitch_rate_and_yaw_rate_async( - float pitch_rate_deg_s, float yaw_rate_deg_s, Gimbal::ResultCallback callback) = 0; - - virtual Gimbal::Result set_mode(const Gimbal::GimbalMode gimbal_mode) = 0; - virtual void - set_mode_async(const Gimbal::GimbalMode gimbal_mode, Gimbal::ResultCallback callback) = 0; - - virtual Gimbal::Result - set_roi_location(double latitude_deg, double longitude_deg, float altitude_m) = 0; - virtual void set_roi_location_async( - double latitude_deg, - double longitude_deg, - float altitude_m, - Gimbal::ResultCallback callback) = 0; - - virtual Gimbal::Result take_control(Gimbal::ControlMode control_mode) = 0; - virtual void - take_control_async(Gimbal::ControlMode control_mode, Gimbal::ResultCallback callback) = 0; - - virtual Gimbal::Result release_control() = 0; - virtual void release_control_async(Gimbal::ResultCallback callback) = 0; - - virtual Gimbal::ControlStatus control() = 0; - virtual void control_async(Gimbal::ControlCallback callback) = 0; - - virtual Gimbal::Attitude attitude() = 0; - virtual void attitude_async(Gimbal::AttitudeCallback callback) = 0; - -protected: - SystemImpl& _system_impl; -}; - -} // namespace mavsdk diff --git a/src/mavsdk/plugins/gimbal/gimbal_protocol_v1.cpp b/src/mavsdk/plugins/gimbal/gimbal_protocol_v1.cpp deleted file mode 100644 index 0f563b208a..0000000000 --- a/src/mavsdk/plugins/gimbal/gimbal_protocol_v1.cpp +++ /dev/null @@ -1,250 +0,0 @@ -#include -#include -#include "gimbal_protocol_v1.h" -#include "gimbal_impl.h" -#include "unused.h" - -namespace mavsdk { - -GimbalProtocolV1::GimbalProtocolV1(SystemImpl& system_impl) : GimbalProtocolBase(system_impl) {} - -GimbalProtocolV1::~GimbalProtocolV1() -{ - _system_impl.remove_call_every(_control_cookie); -} - -Gimbal::Result GimbalProtocolV1::set_angles(float roll_deg, float pitch_deg, float yaw_deg) -{ - MavlinkCommandSender::CommandLong command{}; - - command.command = MAV_CMD_DO_MOUNT_CONTROL; - command.params.maybe_param1 = pitch_deg; - command.params.maybe_param2 = roll_deg; - command.params.maybe_param3 = yaw_deg; - command.params.maybe_param7 = static_cast(MAV_MOUNT_MODE_MAVLINK_TARGETING); - command.target_component_id = _system_impl.get_autopilot_id(); - - return GimbalImpl::gimbal_result_from_command_result(_system_impl.send_command(command)); -} - -void GimbalProtocolV1::set_angles_async( - float roll_deg, float pitch_deg, float yaw_deg, Gimbal::ResultCallback callback) -{ - MavlinkCommandSender::CommandLong command{}; - - command.command = MAV_CMD_DO_MOUNT_CONTROL; - command.params.maybe_param1 = pitch_deg; - command.params.maybe_param2 = roll_deg; - command.params.maybe_param3 = yaw_deg; - command.params.maybe_param7 = static_cast(MAV_MOUNT_MODE_MAVLINK_TARGETING); - command.target_component_id = _system_impl.get_autopilot_id(); - - _system_impl.send_command_async( - command, [callback](MavlinkCommandSender::Result command_result, float progress) { - UNUSED(progress); - GimbalImpl::receive_command_result(command_result, callback); - }); -} - -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) -{ - UNUSED(pitch_rate_deg_s); - UNUSED(yaw_rate_deg_s); - return Gimbal::Result::Unsupported; -} - -void GimbalProtocolV1::set_pitch_rate_and_yaw_rate_async( - float pitch_rate_deg_s, float yaw_rate_deg_s, Gimbal::ResultCallback callback) -{ - UNUSED(pitch_rate_deg_s); - UNUSED(yaw_rate_deg_s); - - if (callback) { - auto temp_callback = callback; - _system_impl.call_user_callback( - [temp_callback]() { temp_callback(Gimbal::Result::Unsupported); }); - } -} - -Gimbal::Result GimbalProtocolV1::set_mode(const Gimbal::GimbalMode gimbal_mode) -{ - MavlinkCommandSender::CommandInt command{}; - - // Correct here would actually be to: - // - set yaw stabilize / param4 to 0 as usual - // - set param7/paramz to 2. - command.command = - MAV_CMD_DO_MOUNT_CONFIGURE; // Mission command to configure a camera or antenna mount - command.params.maybe_param1 = - static_cast(MAV_MOUNT_MODE_MAVLINK_TARGETING); // Mount operation mode - command.params.maybe_param2 = 0.0f; // stabilize roll - command.params.maybe_param3 = 0.0f; // stabilize pitch - command.params.maybe_param4 = - to_float_gimbal_mode(gimbal_mode); // stabilize yaw (1 = yes, 0 = no) - command.params.x = 0; // roll input - command.params.y = 0; // pitch input - command.params.maybe_z = - 2.0f; // yaw input (0 = angle body frame, 1 = angular rate, 2 = angle absolute frame) - command.target_component_id = _system_impl.get_autopilot_id(); - - return GimbalImpl::gimbal_result_from_command_result(_system_impl.send_command(command)); -} - -void GimbalProtocolV1::set_mode_async( - const Gimbal::GimbalMode gimbal_mode, Gimbal::ResultCallback callback) -{ - MavlinkCommandSender::CommandInt command{}; - - // Correct here would actually be to: - // - set yaw stabilize / param4 to 0 as usual - // - set param7/paramz to 2. - command.command = MAV_CMD_DO_MOUNT_CONFIGURE; - command.params.maybe_param1 = - static_cast(MAV_MOUNT_MODE_MAVLINK_TARGETING); // Mount operation mode - command.params.maybe_param2 = 0.0f; // stabilize roll - command.params.maybe_param3 = 0.0f; // stabilize pitch - command.params.maybe_param4 = - to_float_gimbal_mode(gimbal_mode); // stabilize yaw (1 = yes, 0 = no) - command.params.x = 0; // roll input - command.params.y = 0; // pitch input - command.params.maybe_z = - 2.0f; // yaw input (0 = angle body frame, 1 = angular rate, 2 = angle absolute frame) - command.target_component_id = _system_impl.get_autopilot_id(); - - _system_impl.send_command_async( - command, [callback](MavlinkCommandSender::Result command_result, float progress) { - UNUSED(progress); - GimbalImpl::receive_command_result(command_result, callback); - }); -} - -float GimbalProtocolV1::to_float_gimbal_mode(const Gimbal::GimbalMode gimbal_mode) -{ - switch (gimbal_mode) { - case Gimbal::GimbalMode::YawFollow: - return 0.0f; - case Gimbal::GimbalMode::YawLock: - return 1.0f; - default: - return 0.0f; - } -} - -Gimbal::Result -GimbalProtocolV1::set_roi_location(double latitude_deg, double longitude_deg, float altitude_m) -{ - MavlinkCommandSender::CommandInt command{}; - - command.command = MAV_CMD_DO_SET_ROI_LOCATION; - command.params.x = static_cast(std::round(latitude_deg * 1e7)); - command.params.y = static_cast(std::round(longitude_deg * 1e7)); - command.params.maybe_z = altitude_m; - command.target_component_id = _system_impl.get_autopilot_id(); - - return GimbalImpl::gimbal_result_from_command_result(_system_impl.send_command(command)); -} - -void GimbalProtocolV1::set_roi_location_async( - double latitude_deg, double longitude_deg, float altitude_m, Gimbal::ResultCallback callback) -{ - MavlinkCommandSender::CommandInt command{}; - - command.command = MAV_CMD_DO_SET_ROI_LOCATION; - command.params.x = static_cast(std::round(latitude_deg * 1e7)); - command.params.y = static_cast(std::round(longitude_deg * 1e7)); - command.params.maybe_z = altitude_m; - command.target_component_id = _system_impl.get_autopilot_id(); - - _system_impl.send_command_async( - command, [callback](MavlinkCommandSender::Result command_result, float progress) { - UNUSED(progress); - GimbalImpl::receive_command_result(command_result, callback); - }); -} - -Gimbal::Result GimbalProtocolV1::take_control(Gimbal::ControlMode control_mode) -{ - _current_control_status.control_mode = control_mode; - return Gimbal::Result::Success; -} - -void GimbalProtocolV1::take_control_async( - Gimbal::ControlMode control_mode, Gimbal::ResultCallback callback) -{ - _current_control_status.control_mode = control_mode; - - if (callback) { - _system_impl.call_user_callback([callback]() { callback(Gimbal::Result::Success); }); - } -} - -Gimbal::Result GimbalProtocolV1::release_control() -{ - _current_control_status.control_mode = Gimbal::ControlMode::None; - return Gimbal::Result::Success; -} - -void GimbalProtocolV1::release_control_async(Gimbal::ResultCallback callback) -{ - _current_control_status.control_mode = Gimbal::ControlMode::None; - - if (callback) { - _system_impl.call_user_callback([callback]() { callback(Gimbal::Result::Success); }); - } -} - -Gimbal::ControlStatus GimbalProtocolV1::control() -{ - return _current_control_status; -} - -void GimbalProtocolV1::control_async(Gimbal::ControlCallback callback) -{ - if (_control_callback == nullptr && callback != nullptr) { - _control_callback = callback; - _control_cookie = _system_impl.add_call_every( - [this]() { _control_callback(_current_control_status); }, 1.0); - - } else if (_control_callback != nullptr && callback == nullptr) { - _control_callback = callback; - _system_impl.remove_call_every(_control_cookie); - - } else { - _control_callback = callback; - } -} - -Gimbal::Attitude GimbalProtocolV1::attitude() -{ - return _current_attitude; -} - -void GimbalProtocolV1::attitude_async(Gimbal::AttitudeCallback callback) -{ - if (_attitude_callback == nullptr && callback != nullptr) { - _attitude_callback = callback; - _attitude_cookie = - _system_impl.add_call_every([this]() { _attitude_callback(_current_attitude); }, 1.0); - - } else if (_attitude_callback != nullptr && callback == nullptr) { - _attitude_callback = callback; - _system_impl.remove_call_every(_attitude_cookie); - - } else { - _attitude_callback = callback; - } -} - -} // namespace mavsdk diff --git a/src/mavsdk/plugins/gimbal/gimbal_protocol_v1.h b/src/mavsdk/plugins/gimbal/gimbal_protocol_v1.h deleted file mode 100644 index 91707f131c..0000000000 --- a/src/mavsdk/plugins/gimbal/gimbal_protocol_v1.h +++ /dev/null @@ -1,67 +0,0 @@ -#pragma once - -#include -#include - -#include "plugins/gimbal/gimbal.h" -#include "gimbal_protocol_base.h" - -namespace mavsdk { - -class GimbalProtocolV1 : public GimbalProtocolBase { -public: - 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; - - Gimbal::Result - set_pitch_rate_and_yaw_rate(float pitch_rate_deg_s, float yaw_rate_deg_s) override; - - void set_pitch_rate_and_yaw_rate_async( - float pitch_rate_deg_s, float yaw_rate_deg_s, Gimbal::ResultCallback callback) override; - - Gimbal::Result set_mode(const Gimbal::GimbalMode gimbal_mode) override; - void - set_mode_async(const Gimbal::GimbalMode gimbal_mode, Gimbal::ResultCallback callback) override; - - Gimbal::Result - set_roi_location(double latitude_deg, double longitude_deg, float altitude_m) override; - void set_roi_location_async( - double latitude_deg, - double longitude_deg, - float altitude_m, - Gimbal::ResultCallback callback) override; - - Gimbal::Result take_control(Gimbal::ControlMode control_mode) override; - void - take_control_async(Gimbal::ControlMode control_mode, Gimbal::ResultCallback callback) override; - - Gimbal::Result release_control() override; - void release_control_async(Gimbal::ResultCallback callback) override; - - Gimbal::ControlStatus control() override; - void control_async(Gimbal::ControlCallback callback) override; - - Gimbal::Attitude attitude() override; - void attitude_async(Gimbal::AttitudeCallback callback) override; - -private: - static float to_float_gimbal_mode(const Gimbal::GimbalMode gimbal_mode); - - Gimbal::ControlStatus _current_control_status{Gimbal::ControlMode::None, 0, 0, 0, 0}; - Gimbal::ControlCallback _control_callback; - CallEveryHandler::Cookie _control_cookie{}; - - Gimbal::Attitude _current_attitude{}; - Gimbal::AttitudeCallback _attitude_callback{}; - CallEveryHandler::Cookie _attitude_cookie{}; -}; - -} // namespace mavsdk diff --git a/src/mavsdk/plugins/gimbal/gimbal_protocol_v2.cpp b/src/mavsdk/plugins/gimbal/gimbal_protocol_v2.cpp deleted file mode 100644 index 4f1eee51e2..0000000000 --- a/src/mavsdk/plugins/gimbal/gimbal_protocol_v2.cpp +++ /dev/null @@ -1,469 +0,0 @@ -#include "gimbal_protocol_v2.h" -#include "gimbal_impl.h" -#include "math_conversions.h" -#include "mavlink_address.h" -#include "mavsdk_math.h" -#include -#include -#include - -namespace mavsdk { - -GimbalProtocolV2::GimbalProtocolV2( - SystemImpl& system_impl, - const mavlink_gimbal_manager_information_t& information, - uint8_t gimbal_manager_sysid, - uint8_t gimbal_manager_compid) : - GimbalProtocolBase(system_impl), - _gimbal_device_id(information.gimbal_device_id), - _gimbal_manager_sysid(gimbal_manager_sysid), - _gimbal_manager_compid(gimbal_manager_compid) -{ - std::lock_guard lock(_mutex); - - _system_impl.register_mavlink_message_handler( - MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS, - [this](const mavlink_message_t& message) { process_gimbal_manager_status(message); }, - this); - - _system_impl.register_mavlink_message_handler( - MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, - [this](const mavlink_message_t& message) { - process_gimbal_device_attitude_status(message); - }, - this); - - _system_impl.register_mavlink_message_handler( - MAVLINK_MSG_ID_ATTITUDE, - [this](const mavlink_message_t& message) { process_attitude(message); }, - this); -} - -GimbalProtocolV2::~GimbalProtocolV2() -{ - std::lock_guard lock(_mutex); - - _system_impl.unregister_all_mavlink_message_handlers(this); -} - -void GimbalProtocolV2::process_gimbal_manager_status(const mavlink_message_t& message) -{ - mavlink_gimbal_manager_status_t status; - mavlink_msg_gimbal_manager_status_decode(&message, &status); - - std::lock_guard lock(_mutex); - - if (status.primary_control_sysid == static_cast(_system_impl.get_own_system_id()) && - status.primary_control_compid == static_cast(_system_impl.get_own_component_id())) { - _current_control_status.control_mode = Gimbal::ControlMode::Primary; - } else if ( - status.secondary_control_sysid == static_cast(_system_impl.get_own_system_id()) && - status.secondary_control_compid == static_cast(_system_impl.get_own_component_id())) { - _current_control_status.control_mode = Gimbal::ControlMode::Secondary; - } else { - _current_control_status.control_mode = Gimbal::ControlMode::None; - } - - _current_control_status.sysid_primary_control = status.primary_control_sysid; - _current_control_status.compid_primary_control = status.primary_control_compid; - _current_control_status.sysid_secondary_control = status.secondary_control_sysid; - _current_control_status.compid_secondary_control = status.secondary_control_compid; - - if (_control_callback) { - // The queue is called outside of this class. - _control_callback(_current_control_status); - } -} - -void GimbalProtocolV2::process_gimbal_device_attitude_status(const mavlink_message_t& message) -{ - mavlink_gimbal_device_attitude_status_t attitude_status; - mavlink_msg_gimbal_device_attitude_status_decode(&message, &attitude_status); - - // By default, we assume it's in vehicle/forward frame. - bool is_in_forward_frame = true; - - if (attitude_status.flags & GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME || - attitude_status.flags & GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME) { - // Flags are set correctly according to newer spec, so we can use it. - if (attitude_status.flags & GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME) { - is_in_forward_frame = false; - } - } else { - // Neither of the flags indicating the frame are set, we fallback to previous way - // which depends on lock flag. - if (attitude_status.flags & GIMBAL_DEVICE_FLAGS_YAW_LOCK) { - is_in_forward_frame = false; - } - } - - // Reset to defaults (e.g. NaN) first. - _current_attitude = {}; - - if (is_in_forward_frame) { - _current_attitude.quaternion_forward.w = attitude_status.q[0]; - _current_attitude.quaternion_forward.x = attitude_status.q[1]; - _current_attitude.quaternion_forward.y = attitude_status.q[2]; - _current_attitude.quaternion_forward.z = attitude_status.q[3]; - - auto quaternion_forward = Quaternion{}; - quaternion_forward.w = attitude_status.q[0]; - quaternion_forward.x = attitude_status.q[1]; - quaternion_forward.y = attitude_status.q[2]; - quaternion_forward.z = attitude_status.q[3]; - const auto euler_angle_forward = to_euler_angle_from_quaternion(quaternion_forward); - - _current_attitude.euler_angle_forward.roll_deg = euler_angle_forward.roll_deg; - _current_attitude.euler_angle_forward.pitch_deg = euler_angle_forward.pitch_deg; - _current_attitude.euler_angle_forward.yaw_deg = euler_angle_forward.yaw_deg; - - _current_attitude.timestamp_us = attitude_status.time_boot_ms * 1000; - - // Calculate angle relative to North as well - if (!std::isnan(_vehicle_yaw_rad)) { - auto rotation = - to_quaternion_from_euler_angle(EulerAngle{0, 0, to_deg_from_rad(_vehicle_yaw_rad)}); - auto quaternion_north = rotation * quaternion_forward; - - _current_attitude.quaternion_north.w = quaternion_north.w; - _current_attitude.quaternion_north.x = quaternion_north.x; - _current_attitude.quaternion_north.y = quaternion_north.y; - _current_attitude.quaternion_north.z = quaternion_north.z; - - const auto euler_angle_north = to_euler_angle_from_quaternion(quaternion_north); - _current_attitude.euler_angle_north.roll_deg = euler_angle_north.roll_deg; - _current_attitude.euler_angle_north.pitch_deg = euler_angle_north.pitch_deg; - _current_attitude.euler_angle_north.yaw_deg = euler_angle_north.yaw_deg; - } - - } else { - _current_attitude.quaternion_north.w = attitude_status.q[0]; - _current_attitude.quaternion_north.x = attitude_status.q[1]; - _current_attitude.quaternion_north.y = attitude_status.q[2]; - _current_attitude.quaternion_north.z = attitude_status.q[3]; - - auto quaternion_north = Quaternion{}; - quaternion_north.w = attitude_status.q[0]; - quaternion_north.x = attitude_status.q[1]; - quaternion_north.y = attitude_status.q[2]; - quaternion_north.z = attitude_status.q[3]; - const auto euler_angle_north = to_euler_angle_from_quaternion(quaternion_north); - - _current_attitude.euler_angle_north.roll_deg = euler_angle_north.roll_deg; - _current_attitude.euler_angle_north.pitch_deg = euler_angle_north.pitch_deg; - _current_attitude.euler_angle_north.yaw_deg = euler_angle_north.yaw_deg; - - // Calculate angle relative to forward as well - if (!std::isnan(_vehicle_yaw_rad)) { - auto rotation = to_quaternion_from_euler_angle( - EulerAngle{0, 0, -to_deg_from_rad(_vehicle_yaw_rad)}); - auto quaternion_forward = rotation * quaternion_north; - - _current_attitude.quaternion_forward.w = quaternion_forward.w; - _current_attitude.quaternion_forward.x = quaternion_forward.x; - _current_attitude.quaternion_forward.y = quaternion_forward.y; - _current_attitude.quaternion_forward.z = quaternion_forward.z; - - const auto euler_angle_forward = to_euler_angle_from_quaternion(quaternion_forward); - _current_attitude.euler_angle_forward.roll_deg = euler_angle_forward.roll_deg; - _current_attitude.euler_angle_forward.pitch_deg = euler_angle_forward.pitch_deg; - _current_attitude.euler_angle_forward.yaw_deg = euler_angle_forward.yaw_deg; - } - } - - _current_attitude.angular_velocity.roll_rad_s = attitude_status.angular_velocity_x; - _current_attitude.angular_velocity.pitch_rad_s = attitude_status.angular_velocity_y; - _current_attitude.angular_velocity.yaw_rad_s = attitude_status.angular_velocity_z; - - if (_attitude_callback) { - // The queue is called outside of this class. - _attitude_callback(_current_attitude); - } -} - -void GimbalProtocolV2::process_attitude(const mavlink_message_t& message) -{ - mavlink_attitude_t attitude; - mavlink_msg_attitude_decode(&message, &attitude); - - std::lock_guard lock(_mutex); - - _vehicle_yaw_rad = attitude.yaw; -} - -Gimbal::Result GimbalProtocolV2::set_angles(float roll_deg, float pitch_deg, float yaw_deg) -{ - 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); - - float quaternion[4]; - mavlink_euler_to_quaternion(roll_rad, pitch_rad, yaw_rad, quaternion); - - std::lock_guard lock(_mutex); - - const uint32_t flags = - GIMBAL_MANAGER_FLAGS_ROLL_LOCK | GIMBAL_MANAGER_FLAGS_PITCH_LOCK | - ((_gimbal_mode == Gimbal::GimbalMode::YawLock) ? GIMBAL_MANAGER_FLAGS_YAW_LOCK : 0); - - return _system_impl.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { - mavlink_message_t message; - mavlink_msg_gimbal_manager_set_attitude_pack_chan( - mavlink_address.system_id, - mavlink_address.component_id, - channel, - &message, - _gimbal_manager_sysid, - _gimbal_manager_compid, - flags, - _gimbal_device_id, - quaternion, - NAN, - NAN, - NAN); - return message; - }) ? - Gimbal::Result::Success : - 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); - - std::lock_guard lock(_mutex); - - 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_angles(0.0f, pitch_deg, yaw_deg); - - if (callback) { - _system_impl.call_user_callback([callback, result]() { callback(result); }); - } -} - -Gimbal::Result -GimbalProtocolV2::set_pitch_rate_and_yaw_rate(float pitch_rate_deg_s, float yaw_rate_deg_s) -{ - std::lock_guard lock(_mutex); - - const uint32_t flags = - GIMBAL_MANAGER_FLAGS_ROLL_LOCK | GIMBAL_MANAGER_FLAGS_PITCH_LOCK | - ((_gimbal_mode == Gimbal::GimbalMode::YawLock) ? GIMBAL_MANAGER_FLAGS_YAW_LOCK : 0); - - const float quaternion[4] = {NAN, NAN, NAN, NAN}; - - return _system_impl.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { - mavlink_message_t message; - mavlink_msg_gimbal_manager_set_attitude_pack_chan( - mavlink_address.system_id, - mavlink_address.component_id, - channel, - &message, - _gimbal_manager_sysid, - _gimbal_manager_compid, - flags, - _gimbal_device_id, - quaternion, - 0.0f, - to_rad_from_deg(pitch_rate_deg_s), - to_rad_from_deg(yaw_rate_deg_s)); - return message; - }) ? - Gimbal::Result::Success : - Gimbal::Result::Error; -} - -void GimbalProtocolV2::set_pitch_rate_and_yaw_rate_async( - float pitch_rate_deg_s, float yaw_rate_deg_s, Gimbal::ResultCallback callback) -{ - // Sending the message should be quick and we can just do that straighaway. - Gimbal::Result result = set_pitch_rate_and_yaw_rate(pitch_rate_deg_s, yaw_rate_deg_s); - - std::lock_guard lock(_mutex); - - if (callback) { - auto temp_callback = callback; - _system_impl.call_user_callback([temp_callback, result]() { temp_callback(result); }); - } -} - -Gimbal::Result GimbalProtocolV2::set_mode(const Gimbal::GimbalMode gimbal_mode) -{ - std::lock_guard lock(_mutex); - - _gimbal_mode = gimbal_mode; - return Gimbal::Result::Success; -} - -void GimbalProtocolV2::set_mode_async( - const Gimbal::GimbalMode gimbal_mode, Gimbal::ResultCallback callback) -{ - std::lock_guard lock(_mutex); - - _gimbal_mode = gimbal_mode; - - if (callback) { - _system_impl.call_user_callback([callback]() { callback(Gimbal::Result::Success); }); - } -} - -Gimbal::Result -GimbalProtocolV2::set_roi_location(double latitude_deg, double longitude_deg, float altitude_m) -{ - MavlinkCommandSender::CommandInt command{}; - - command.command = MAV_CMD_DO_SET_ROI_LOCATION; - command.params.x = static_cast(std::round(latitude_deg * 1e7)); - command.params.y = static_cast(std::round(longitude_deg * 1e7)); - command.params.maybe_z = altitude_m; - command.target_system_id = _gimbal_manager_sysid; - command.target_component_id = _gimbal_manager_compid; - - return GimbalImpl::gimbal_result_from_command_result(_system_impl.send_command(command)); -} - -void GimbalProtocolV2::set_roi_location_async( - double latitude_deg, double longitude_deg, float altitude_m, Gimbal::ResultCallback callback) -{ - std::lock_guard lock(_mutex); - - MavlinkCommandSender::CommandInt command{}; - - command.command = MAV_CMD_DO_SET_ROI_LOCATION; - command.params.x = static_cast(std::round(latitude_deg * 1e7)); - command.params.y = static_cast(std::round(longitude_deg * 1e7)); - command.params.maybe_z = altitude_m; - command.target_system_id = _gimbal_manager_sysid; - command.target_component_id = _gimbal_manager_compid; - - _system_impl.send_command_async( - command, [callback](MavlinkCommandSender::Result result, float) { - GimbalImpl::receive_command_result(result, callback); - }); -} - -Gimbal::Result GimbalProtocolV2::take_control(Gimbal::ControlMode control_mode) -{ - auto prom = std::promise(); - auto fut = prom.get_future(); - - take_control_async(control_mode, [&prom](Gimbal::Result result) { prom.set_value(result); }); - - return fut.get(); -} - -void GimbalProtocolV2::take_control_async( - Gimbal::ControlMode control_mode, Gimbal::ResultCallback callback) -{ - std::lock_guard lock(_mutex); - - if (control_mode == Gimbal::ControlMode::None) { - release_control_async(callback); - return; - } else if (control_mode == Gimbal::ControlMode::Secondary) { - _system_impl.call_user_callback([callback]() { callback(Gimbal::Result::Unsupported); }); - LogErr() << "Gimbal secondary control is not implemented yet!"; - return; - } - - float own_sysid = _system_impl.get_own_system_id(); - float own_compid = _system_impl.get_own_component_id(); - - MavlinkCommandSender::CommandLong command{}; - - command.command = MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE; - command.params.maybe_param1 = - control_mode == Gimbal::ControlMode::Primary ? own_sysid : -3.0f; // sysid primary control - command.params.maybe_param2 = - control_mode == Gimbal::ControlMode::Primary ? own_compid : -3.0f; // compid primary control - command.params.maybe_param3 = - control_mode == Gimbal::ControlMode::Primary ? own_sysid : -3.0f; // sysid secondary control - command.params.maybe_param4 = control_mode == Gimbal::ControlMode::Primary ? - own_compid : - -3.0f; // compid secondary control - - command.params.maybe_param7 = _gimbal_device_id; - command.target_system_id = _gimbal_manager_sysid; - command.target_component_id = _gimbal_manager_compid; - - _system_impl.send_command_async( - command, [callback](MavlinkCommandSender::Result result, float) { - GimbalImpl::receive_command_result(result, callback); - }); -} - -Gimbal::Result GimbalProtocolV2::release_control() -{ - auto prom = std::promise(); - auto fut = prom.get_future(); - - release_control_async([&prom](Gimbal::Result result) { prom.set_value(result); }); - - return fut.get(); -} - -void GimbalProtocolV2::release_control_async(Gimbal::ResultCallback callback) -{ - std::lock_guard lock(_mutex); - - MavlinkCommandSender::CommandLong command{}; - - command.command = MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE; - command.params.maybe_param1 = -3.0f; // sysid primary control - command.params.maybe_param2 = -3.0f; // compid primary control - command.params.maybe_param3 = -3.0f; // sysid secondary control - command.params.maybe_param4 = -3.0f; // compid secondary control - command.params.maybe_param7 = _gimbal_device_id; - command.target_system_id = _gimbal_manager_sysid; - command.target_component_id = _gimbal_manager_compid; - - _system_impl.send_command_async( - command, [callback](MavlinkCommandSender::Result result, float) { - GimbalImpl::receive_command_result(result, callback); - }); -} - -Gimbal::ControlStatus GimbalProtocolV2::control() -{ - std::lock_guard lock(_mutex); - - return _current_control_status; -} - -void GimbalProtocolV2::control_async(Gimbal::ControlCallback callback) -{ - std::lock_guard lock(_mutex); - - _control_callback = callback; -} - -Gimbal::Attitude GimbalProtocolV2::attitude() -{ - std::lock_guard lock(_mutex); - - return _current_attitude; -} - -void GimbalProtocolV2::attitude_async(Gimbal::AttitudeCallback callback) -{ - std::lock_guard lock(_mutex); - - _attitude_callback = callback; -} - -} // namespace mavsdk diff --git a/src/mavsdk/plugins/gimbal/gimbal_protocol_v2.h b/src/mavsdk/plugins/gimbal/gimbal_protocol_v2.h deleted file mode 100644 index 42f533daa4..0000000000 --- a/src/mavsdk/plugins/gimbal/gimbal_protocol_v2.h +++ /dev/null @@ -1,76 +0,0 @@ -#pragma once - -#include "plugins/gimbal/gimbal.h" -#include "gimbal_protocol_base.h" -#include - -namespace mavsdk { - -class GimbalProtocolV2 : public GimbalProtocolBase { -public: - GimbalProtocolV2( - SystemImpl& system_impl, - const mavlink_gimbal_manager_information_t& information, - uint8_t gimbal_manager_sysid, - 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; - - Gimbal::Result - set_pitch_rate_and_yaw_rate(float pitch_rate_deg_s, float yaw_rate_deg_s) override; - - void set_pitch_rate_and_yaw_rate_async( - float pitch_rate_deg_s, float yaw_rate_deg_s, Gimbal::ResultCallback callback) override; - - Gimbal::Result set_mode(const Gimbal::GimbalMode gimbal_mode) override; - void - set_mode_async(const Gimbal::GimbalMode gimbal_mode, Gimbal::ResultCallback callback) override; - - Gimbal::Result - set_roi_location(double latitude_deg, double longitude_deg, float altitude_m) override; - void set_roi_location_async( - double latitude_deg, - double longitude_deg, - float altitude_m, - Gimbal::ResultCallback callback) override; - - Gimbal::Result take_control(Gimbal::ControlMode control_mode) override; - void - take_control_async(Gimbal::ControlMode control_mode, Gimbal::ResultCallback callback) override; - - Gimbal::Result release_control() override; - void release_control_async(Gimbal::ResultCallback callback) override; - - Gimbal::ControlStatus control() override; - void control_async(Gimbal::ControlCallback callback) override; - - Gimbal::Attitude attitude() override; - void attitude_async(Gimbal::AttitudeCallback callback) override; - -private: - void set_gimbal_information(const mavlink_gimbal_manager_information_t& information); - void process_gimbal_manager_status(const mavlink_message_t& message); - void process_gimbal_device_attitude_status(const mavlink_message_t& message); - void process_attitude(const mavlink_message_t& message); - - uint8_t _gimbal_device_id; - uint8_t _gimbal_manager_sysid; - uint8_t _gimbal_manager_compid; - - std::mutex _mutex; - Gimbal::GimbalMode _gimbal_mode{Gimbal::GimbalMode::YawFollow}; - Gimbal::ControlStatus _current_control_status{Gimbal::ControlMode::None, 0, 0, 0, 0}; - Gimbal::ControlCallback _control_callback; - Gimbal::Attitude _current_attitude{}; - Gimbal::AttitudeCallback _attitude_callback; - float _vehicle_yaw_rad{NAN}; -}; - -} // namespace mavsdk