From d1083610f8f345a19185444662809bcd4c0c5ca0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C4=90=E1=BB=97=20B=E1=BA=A3o=20Long?= Date: Fri, 17 Apr 2026 07:26:50 +0000 Subject: [PATCH 1/3] Ready for committing to PX4 community --- msg/CMakeLists.txt | 3 + msg/ExternalGimbalManagerInformation.msg | 33 ++ msg/ExternalGimbalManagerSetAttitude.msg | 30 ++ msg/ExternalGimbalManagerStatus.msg | 14 + msg/GimbalDeviceSetAttitude.msg | 5 + msg/versioned/VehicleCommand.msg | 2 + src/modules/gimbal/gimbal.cpp | 33 +- src/modules/gimbal/gimbal_params.h | 15 +- src/modules/gimbal/gimbal_params.yaml | 12 +- src/modules/gimbal/input_mavlink.cpp | 79 +++-- src/modules/gimbal/input_mavlink.h | 10 + src/modules/gimbal/input_rc.cpp | 2 +- src/modules/gimbal/input_test.cpp | 12 +- src/modules/gimbal/output_mavlink.cpp | 309 +++++++++++++++++- src/modules/gimbal/output_mavlink.h | 94 ++++++ src/modules/mavlink/mavlink_main.cpp | 66 +++- src/modules/mavlink/mavlink_main.h | 3 +- src/modules/mavlink/mavlink_messages.cpp | 4 + src/modules/mavlink/mavlink_receiver.cpp | 64 ++++ src/modules/mavlink/mavlink_receiver.h | 6 + .../EXTERNAL_GIMBAL_MANAGER_SET_ATTITUDE.hpp | 61 ++++ 21 files changed, 806 insertions(+), 51 deletions(-) create mode 100644 msg/ExternalGimbalManagerInformation.msg create mode 100644 msg/ExternalGimbalManagerSetAttitude.msg create mode 100644 msg/ExternalGimbalManagerStatus.msg create mode 100644 src/modules/mavlink/streams/EXTERNAL_GIMBAL_MANAGER_SET_ATTITUDE.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 3ecb1ee59769..9b58de78c2aa 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -87,6 +87,9 @@ set(msg_files EstimatorStates.msg EstimatorStatus.msg EstimatorStatusFlags.msg + ExternalGimbalManagerInformation.msg + ExternalGimbalManagerSetAttitude.msg + ExternalGimbalManagerStatus.msg versioned/Event.msg FigureEightStatus.msg FailsafeFlags.msg diff --git a/msg/ExternalGimbalManagerInformation.msg b/msg/ExternalGimbalManagerInformation.msg new file mode 100644 index 000000000000..770315c56dd1 --- /dev/null +++ b/msg/ExternalGimbalManagerInformation.msg @@ -0,0 +1,33 @@ +#Bridge-private information received from external gimbal manager +uint64 timestamp # time since system start (microseconds) + +uint8 source_system +uint8 source_component + +uint32 cap_flags + +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_RETRACT = 1 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL = 2 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_AXIS = 4 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_FOLLOW = 8 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK = 16 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS = 32 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_FOLLOW = 64 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK = 128 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS = 256 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_FOLLOW = 512 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK = 1024 +uint32 GIMBAL_MANAGER_CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048 +uint32 GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_LOCAL = 65536 +uint32 GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL = 131072 + +uint8 gimbal_device_id + +float32 roll_min # [rad] +float32 roll_max # [rad] + +float32 pitch_min # [rad] +float32 pitch_max # [rad] + +float32 yaw_min # [rad] +float32 yaw_max # [rad] diff --git a/msg/ExternalGimbalManagerSetAttitude.msg b/msg/ExternalGimbalManagerSetAttitude.msg new file mode 100644 index 000000000000..2ff553861d76 --- /dev/null +++ b/msg/ExternalGimbalManagerSetAttitude.msg @@ -0,0 +1,30 @@ +#Bridge private set-attitude command for an external gimbal manager +uint64 timestamp # time since system start (microseconds) + +uint8 origin_sysid +uint8 origin_compid + +uint8 target_system +uint8 target_component + +uint32 EXTERNAL_GIMBAL_MANAGER_FLAGS_RETRACT = 1 +uint32 EXTERNAL_GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 +uint32 EXTERNAL_GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 +uint32 EXTERNAL_GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 +uint32 EXTERNAL_GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 +uint32 EXTERNAL_GIMBAL_MANAGER_FLAGS_YAW_IN_VEHICLE_FRAME = 32 +uint32 EXTERNAL_GIMBAL_MANAGER_FLAGS_YAW_IN_EARTH_FRAME = 64 +uint32 EXTERNAL_GIMBAL_MANAGER_FLAGS_ACCEPTS_YAW_IN_EARTH_FRAME = 128 +uint32 EXTERNAL_GIMBAL_MANAGER_FLAGS_RC_EXCLUSIVE = 256 +uint32 EXTERNAL_GIMBAL_MANAGER_FLAGS_RC_MIXED = 512 + +uint32 flags +uint8 gimbal_device_id + +float32[4] q + +float32 angular_velocity_x +float32 angular_velocity_y +float32 angular_velocity_z + +uint8 ORB_QUEUE_LENGTH = 2 diff --git a/msg/ExternalGimbalManagerStatus.msg b/msg/ExternalGimbalManagerStatus.msg new file mode 100644 index 000000000000..252608a22210 --- /dev/null +++ b/msg/ExternalGimbalManagerStatus.msg @@ -0,0 +1,14 @@ +#Bridge-private status received from an external gimbal manager +uint64 timestamp # time since system start (microseconds) + +uint32 flags +uint8 gimbal_device_id + +uint8 primary_control_sysid +uint8 primary_control_compid + +uint8 secondary_control_sysid +uint8 secondary_control_compid + +uint8 source_system +uint8 source_component diff --git a/msg/GimbalDeviceSetAttitude.msg b/msg/GimbalDeviceSetAttitude.msg index f224a23441be..64b9e55c5c3d 100644 --- a/msg/GimbalDeviceSetAttitude.msg +++ b/msg/GimbalDeviceSetAttitude.msg @@ -9,6 +9,11 @@ uint32 GIMBAL_DEVICE_FLAGS_NEUTRAL = 2 uint32 GIMBAL_DEVICE_FLAGS_ROLL_LOCK = 4 uint32 GIMBAL_DEVICE_FLAGS_PITCH_LOCK = 8 uint32 GIMBAL_DEVICE_FLAGS_YAW_LOCK = 16 +uint32 GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME = 32 +uint32 GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME = 64 +uint32 GIMBAL_DEVICE_FLAGS_ACCEPTS_YAW_IN_EARTH_FRAME = 128 +uint32 GIMBAL_DEVICE_FLAGS_RC_EXCLUSIVE = 256 +uint32 GIMBAL_DEVICE_FLAGS_RC_MIXED = 512 float32[4] q diff --git a/msg/versioned/VehicleCommand.msg b/msg/versioned/VehicleCommand.msg index be86cd22df59..a19dcf91905c 100644 --- a/msg/versioned/VehicleCommand.msg +++ b/msg/versioned/VehicleCommand.msg @@ -75,6 +75,8 @@ uint16 VEHICLE_CMD_PREFLIGHT_STORAGE = 245 # Request storage of different parame uint16 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246 # Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.|0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.|Unused|Unused|Unused|Unused|Unused| uint16 VEHICLE_CMD_OBLIQUE_SURVEY=260 # Mission command to set a Camera Auto Mount Pivoting Oblique Survey for this flight. |[m] Camera trigger distance|[ms] Shutter integration time|Camera minimum trigger interval|Number of positions|Roll|Pitch|Unused| uint16 VEHICLE_CMD_DO_SET_STANDARD_MODE=262 # Enable the specified standard MAVLink mode. |MAV_STANDARD_MODE| +uint16 VEHICLE_CMD_GIMBAL_MANAGER_INFORMATION = 280 # Command to ask information about a gimbal manager. +uint16 VEHICLE_CMD_GIMBAL_MANAGER_STATUS = 281 # Command to ask status about a gimbal manager uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information about a low level gimbal. uint16 VEHICLE_CMD_MISSION_START = 300 # Start running a mission. |first_item: the first mission item to run|last_item: the last mission item to run (after this item is run, the mission ends)| diff --git a/src/modules/gimbal/gimbal.cpp b/src/modules/gimbal/gimbal.cpp index 137f38c64407..eac6d4161cb8 100644 --- a/src/modules/gimbal/gimbal.cpp +++ b/src/modules/gimbal/gimbal.cpp @@ -42,6 +42,7 @@ * - PWM * - MAVLink gimbal protocol v1 * - MAVLink gimbal protocol v2 + * - MAVLink gimbal protocol v2 (gimbal manager mount) */ #include @@ -82,7 +83,6 @@ struct ThreadData { }; static ThreadData *g_thread_data = nullptr; - static void usage(); static void update_params(ParameterHandles ¶m_handles, Parameters ¶ms); static bool initialize_params(ParameterHandles ¶m_handles, Parameters ¶ms); @@ -102,6 +102,21 @@ static int gimbal_thread_main(int argc, char *argv[]) return -1; } + // When MNT_MODE_OUT = MNT_MODE_OUT_TO_GIMBAL_MANAGER, + // MNT_MODE_IN = MAVLINK_ROI || MAVLINK_DO_MOUNT || MAVLINK_V2 are NOT ALLOWED + // However, people can access MAVLINK_V2 through MNT_MODE_IN = AUTO. + const bool px4_acts_as_gimbal_manager = (params.mnt_mode_in == MNT_MODE_IN_MAVLINK_ROI) + || (params.mnt_mode_in == MNT_MODE_IN_MAVLINK_DO_MOUNT) + || (params.mnt_mode_in == MNT_MODE_IN_MAVLINK_V2); + + const bool connected_to_external_gimbal_manager = (params.mnt_mode_out == gimbal::MNT_MODE_OUT_TO_GIMBAL_MANAGER); + + // Conflict occurs when PX4 itself acts as a gimbal manager while also communicating with an external gimbal manager. + if (px4_acts_as_gimbal_manager && connected_to_external_gimbal_manager) { + PX4_ERR("MNT_MODE_OUT=MNT_MODE_OUT_TO_GIMBAL_MANAGER is only compatible with MNT_MODE_IN=AUTO or RC."); + return -1; + } + uORB::SubscriptionInterval parameter_update_sub{ORB_ID(parameter_update), 1_s}; thread_running.store(true); g_thread_data = &thread_data; @@ -186,6 +201,13 @@ static int gimbal_thread_main(int argc, char *argv[]) break; + case MNT_MODE_OUT_TO_GIMBAL_MANAGER: //Gimbal manager protocol + thread_data.output_obj = new OutputMavlinkToGimbalManager(params); + + if (!thread_data.output_obj) { alloc_failed = true; } + + break; + default: PX4_ERR("invalid output mode %" PRId32, params.mnt_mode_out); thread_should_exit.store(true); @@ -288,9 +310,11 @@ static int gimbal_thread_main(int argc, char *argv[]) thread_data.control_data, update_result != InputBase::UpdateResult::NoUpdate, thread_data.control_data.device_compid); - // Only publish the mount orientation if the mode is not mavlink v1 or v2 + // Only publish the mount orientation if the mode is not MAVLink-based. // If the gimbal speaks mavlink it publishes its own orientation. - if (params.mnt_mode_out != MNT_MODE_OUT_MAVLINK_V1 && params.mnt_mode_out != MNT_MODE_OUT_MAVLINK_V2) { + if (params.mnt_mode_out != MNT_MODE_OUT_MAVLINK_V1 && + params.mnt_mode_out != MNT_MODE_OUT_MAVLINK_V2 && + params.mnt_mode_out != MNT_MODE_OUT_TO_GIMBAL_MANAGER) { thread_data.output_obj->publish(); } @@ -554,6 +578,7 @@ void update_params(ParameterHandles ¶m_handles, Parameters ¶ms) param_get(param_handles.mav_sysid, ¶ms.mav_sysid); param_get(param_handles.mav_compid, ¶ms.mav_compid); param_get(param_handles.mnt_rate_pitch, ¶ms.mnt_rate_pitch); + param_get(param_handles.mnt_rate_roll, ¶ms.mnt_rate_roll); param_get(param_handles.mnt_rate_yaw, ¶ms.mnt_rate_yaw); param_get(param_handles.mnt_rc_in_mode, ¶ms.mnt_rc_in_mode); param_get(param_handles.mnt_lnd_p_min, ¶ms.mnt_lnd_p_min); @@ -578,6 +603,7 @@ bool initialize_params(ParameterHandles ¶m_handles, Parameters ¶ms) param_handles.mav_sysid = param_find("MAV_SYS_ID"); param_handles.mav_compid = param_find("MAV_COMP_ID"); param_handles.mnt_rate_pitch = param_find("MNT_RATE_PITCH"); + param_handles.mnt_rate_roll = param_find("MNT_RATE_ROLL"); param_handles.mnt_rate_yaw = param_find("MNT_RATE_YAW"); param_handles.mnt_rc_in_mode = param_find("MNT_RC_IN_MODE"); param_handles.mnt_lnd_p_min = param_find("MNT_LND_P_MIN"); @@ -599,6 +625,7 @@ bool initialize_params(ParameterHandles ¶m_handles, Parameters ¶ms) param_handles.mav_sysid == PARAM_INVALID || param_handles.mav_compid == PARAM_INVALID || param_handles.mnt_rate_pitch == PARAM_INVALID || + param_handles.mnt_rate_roll == PARAM_INVALID || param_handles.mnt_rate_yaw == PARAM_INVALID || param_handles.mnt_rc_in_mode == PARAM_INVALID || param_handles.mnt_lnd_p_min == PARAM_INVALID || diff --git a/src/modules/gimbal/gimbal_params.h b/src/modules/gimbal/gimbal_params.h index e480dc41d27d..2551673578e4 100644 --- a/src/modules/gimbal/gimbal_params.h +++ b/src/modules/gimbal/gimbal_params.h @@ -42,17 +42,18 @@ namespace gimbal enum MntModeIn { MNT_MODE_IN_DISABLED = -1, - MNT_MODE_IN_AUTO, // RC and MAVLink gimbal protocol v2 + MNT_MODE_IN_AUTO, // RC and MAVLink gimbal protocol v2 MNT_MODE_IN_RC, - MNT_MODE_IN_MAVLINK_ROI, // MAVLink gimbal protocol v1 (to be deprecated) - MNT_MODE_IN_MAVLINK_DO_MOUNT, // MAVLink gimbal protocol v1 (to be deprecated) - MNT_MODE_IN_MAVLINK_V2 // MAVLink gimbal protocol v2 + MNT_MODE_IN_MAVLINK_ROI, // MAVLink gimbal protocol v1 (to be deprecated) + MNT_MODE_IN_MAVLINK_DO_MOUNT, // MAVLink gimbal protocol v1 (to be deprecated) + MNT_MODE_IN_MAVLINK_V2 // MAVLink gimbal protocol v2 }; enum MntModeOut { MNT_MODE_OUT_AUX = 0, - MNT_MODE_OUT_MAVLINK_V1, // MAVLink gimbal protocol v1 (to be deprecated) - MNT_MODE_OUT_MAVLINK_V2 // MAVLink gimbal protocol v2 + MNT_MODE_OUT_MAVLINK_V1, // MAVLink gimbal protocol v1 (to be deprecated) + MNT_MODE_OUT_MAVLINK_V2, // MAVLink gimbal protocol v2 + MNT_MODE_OUT_TO_GIMBAL_MANAGER // MAVLink gimbal protocol v2 (mount gimbal manager) }; struct Parameters { @@ -71,6 +72,7 @@ struct Parameters { int32_t mav_sysid; int32_t mav_compid; float mnt_rate_pitch; + float mnt_rate_roll; float mnt_rate_yaw; int32_t mnt_rc_in_mode; float mnt_lnd_p_min; @@ -94,6 +96,7 @@ struct ParameterHandles { param_t mav_sysid; param_t mav_compid; param_t mnt_rate_pitch; + param_t mnt_rate_roll; param_t mnt_rate_yaw; param_t mnt_rc_in_mode; param_t mnt_lnd_p_min; diff --git a/src/modules/gimbal/gimbal_params.yaml b/src/modules/gimbal/gimbal_params.yaml index 4b09d359e7de..bdf2d7fa08ae 100644 --- a/src/modules/gimbal/gimbal_params.yaml +++ b/src/modules/gimbal/gimbal_params.yaml @@ -34,9 +34,10 @@ parameters: 0: AUX 1: MAVLink gimbal protocol v1 2: MAVLink gimbal protocol v2 + 3: MAVLink gimbal protocol v3 (mount gimbal manager) default: 0 min: 0 - max: 2 + max: 3 reboot_required: true MNT_MAV_SYSID: description: @@ -158,6 +159,15 @@ parameters: min: 1.0 max: 90.0 unit: deg/s + MNT_RATE_ROLL: + description: + short: Angular roll rate for manual input in degrees/second + long: Full stick input [-1..1] translats to [-roll rate..roll rate]. + type: float + default: 0.0 + min: 0.0 + max: 90.0 + unit: deg/s MNT_RATE_YAW: description: short: Angular yaw rate for manual input in degrees/second diff --git a/src/modules/gimbal/input_mavlink.cpp b/src/modules/gimbal/input_mavlink.cpp index 7091a7bc89db..c05357eb109b 100644 --- a/src/modules/gimbal/input_mavlink.cpp +++ b/src/modules/gimbal/input_mavlink.cpp @@ -531,22 +531,47 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_information(const ControlData InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::update(unsigned int timeout_ms, ControlData &control_data, bool already_active) { + // In MISSION-ONLY mode, do not poll gimbal_manager_set_attitude/manual-control topics + // because they should only be active in FULL Mavlink V2 control mode + px4_pollfd_struct_t polls[5] {}; + int num_poll = 0; + + int idx_gimbal_manager_set_attitude = -1; + int idx_vehicle_roi = -1; + int idx_position_setpoint_triplet = -1; + int idx_vehicle_command = -1; + int idx_gimbal_manager_set_manual_control = -1; + + if (_input_mavlinkv2_mode == MavlinkInputMode::FULL) { + idx_gimbal_manager_set_attitude = num_poll; + polls[num_poll].fd = _gimbal_manager_set_attitude_sub; + polls[num_poll].events = POLLIN; + num_poll++; + } - const int num_poll = 5; - px4_pollfd_struct_t polls[num_poll]; - polls[0].fd = _gimbal_manager_set_attitude_sub; - polls[0].events = POLLIN; - polls[1].fd = _vehicle_roi_sub; - polls[1].events = POLLIN; - polls[2].fd = _position_setpoint_triplet_sub; - polls[2].events = POLLIN; - polls[3].fd = _vehicle_command_sub; - polls[3].events = POLLIN; - polls[4].fd = _gimbal_manager_set_manual_control_sub; - polls[4].events = POLLIN; + idx_vehicle_roi = num_poll; + polls[num_poll].fd = _vehicle_roi_sub; + polls[num_poll].events = POLLIN; + num_poll++; + + idx_position_setpoint_triplet = num_poll; + polls[num_poll].fd = _position_setpoint_triplet_sub; + polls[num_poll].events = POLLIN; + num_poll++; + + idx_vehicle_command = num_poll; + polls[num_poll].fd = _vehicle_command_sub; + polls[num_poll].events = POLLIN; + num_poll++; + + if (_input_mavlinkv2_mode == MavlinkInputMode::FULL) { + idx_gimbal_manager_set_manual_control = num_poll; + polls[num_poll].fd = _gimbal_manager_set_manual_control_sub; + polls[num_poll].events = POLLIN; + num_poll++; + } int poll_timeout = (int) timeout_ms; - hrt_abstime poll_start = hrt_absolute_time(); // If we get a command that we need to handle we exit the loop, otherwise we poll until we @@ -563,16 +588,17 @@ InputMavlinkGimbalV2::update(unsigned int timeout_ms, ControlData &control_data, if (ret <= 0) { // Error, or timeout, give up. exit_loop = true; + break; } - if (polls[0].revents & POLLIN) { + if (idx_gimbal_manager_set_attitude >= 0 && polls[idx_gimbal_manager_set_attitude].revents & POLLIN) { gimbal_manager_set_attitude_s set_attitude; orb_copy(ORB_ID(gimbal_manager_set_attitude), _gimbal_manager_set_attitude_sub, &set_attitude); update_result = _process_set_attitude(control_data, set_attitude); } - if (polls[1].revents & POLLIN) { + if (polls[idx_vehicle_roi].revents & POLLIN) { vehicle_roi_s vehicle_roi; orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &vehicle_roi); @@ -584,7 +610,7 @@ InputMavlinkGimbalV2::update(unsigned int timeout_ms, ControlData &control_data, } // check whether the position setpoint got updated - if (polls[2].revents & POLLIN) { + if (polls[idx_position_setpoint_triplet].revents & POLLIN) { position_setpoint_triplet_s position_setpoint_triplet; orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet); @@ -596,7 +622,7 @@ InputMavlinkGimbalV2::update(unsigned int timeout_ms, ControlData &control_data, } } - if (polls[3].revents & POLLIN) { + if (polls[idx_vehicle_command].revents & POLLIN) { vehicle_command_s vehicle_command; orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command); @@ -607,7 +633,7 @@ InputMavlinkGimbalV2::update(unsigned int timeout_ms, ControlData &control_data, } } - if (polls[4].revents & POLLIN) { + if (idx_gimbal_manager_set_manual_control >= 0 && polls[idx_gimbal_manager_set_manual_control].revents & POLLIN) { gimbal_manager_set_manual_control_s set_manual_control; orb_copy(ORB_ID(gimbal_manager_set_manual_control), _gimbal_manager_set_manual_control_sub, &set_manual_control); @@ -618,11 +644,14 @@ InputMavlinkGimbalV2::update(unsigned int timeout_ms, ControlData &control_data, poll_timeout = timeout_ms - (hrt_absolute_time() - poll_start) / 1000; } - _stream_gimbal_manager_status(control_data); + // Only streams gimbal_manager_status / information when it is in FULL MAVLink V2 control mode + if (_input_mavlinkv2_mode == MavlinkInputMode::FULL) { + _stream_gimbal_manager_status(control_data); - if (_last_device_compid != control_data.device_compid) { - _last_device_compid = control_data.device_compid; - _stream_gimbal_manager_information(control_data); + if (_last_device_compid != control_data.device_compid) { + _last_device_compid = control_data.device_compid; + _stream_gimbal_manager_information(control_data); + } } return update_result; @@ -718,6 +747,12 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ return UpdateResult::NoUpdate; } + // Reject the ownership changing command because MNT_MODE_OUT = MNT_MODE_OUT_TO_GIMBAL_MANAGER handles DO_GIMBAL_MANAGER_CONFIGURE + if (_input_mavlinkv2_mode == MavlinkInputMode::MISSION_ONLY + && vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE) { + return UpdateResult::NoUpdate; + } + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) { // FIXME: Remove me later. For now, we support this for legacy missions // using gimbal v1 protocol. diff --git a/src/modules/gimbal/input_mavlink.h b/src/modules/gimbal/input_mavlink.h index a47af82fc48d..466ac0e216a0 100644 --- a/src/modules/gimbal/input_mavlink.h +++ b/src/modules/gimbal/input_mavlink.h @@ -119,6 +119,16 @@ class InputMavlinkGimbalV2 : public InputBase void _stream_gimbal_manager_status(const ControlData &control_data); void _read_control_data_from_position_setpoint_sub(ControlData &control_data); + // Add-in feature to triggle between a normal and mission-only InputMavlinkV2 + enum class MavlinkInputMode { + FULL = 0, + MISSION_ONLY = 1 + }; + + MavlinkInputMode _input_mavlinkv2_mode = (_parameters.mnt_mode_out == gimbal::MNT_MODE_OUT_TO_GIMBAL_MANAGER) ? + MavlinkInputMode::MISSION_ONLY : + MavlinkInputMode::FULL; + int _vehicle_roi_sub = -1; int _gimbal_manager_set_attitude_sub = -1; int _gimbal_manager_set_manual_control_sub = -1; diff --git a/src/modules/gimbal/input_rc.cpp b/src/modules/gimbal/input_rc.cpp index ab399d10cb56..199b311b18c0 100644 --- a/src/modules/gimbal/input_rc.cpp +++ b/src/modules/gimbal/input_rc.cpp @@ -155,7 +155,7 @@ InputRC::UpdateResult InputRC::_read_control_data_from_subscription(ControlData control_data.type_data.angle.q[2] = NAN; control_data.type_data.angle.q[3] = NAN; - control_data.type_data.angle.angular_velocity[0] = 0.f; + control_data.type_data.angle.angular_velocity[0] = math::radians(_parameters.mnt_rate_roll) * new_aux_values[0]; control_data.type_data.angle.angular_velocity[1] = math::radians(_parameters.mnt_rate_pitch) * new_aux_values[1]; control_data.type_data.angle.angular_velocity[2] = math::radians(_parameters.mnt_rate_yaw) * new_aux_values[2]; diff --git a/src/modules/gimbal/input_test.cpp b/src/modules/gimbal/input_test.cpp index 67dc95251258..12d0113690a8 100644 --- a/src/modules/gimbal/input_test.cpp +++ b/src/modules/gimbal/input_test.cpp @@ -96,12 +96,12 @@ int InputTest::initialize() void InputTest::print_status() const { PX4_INFO("Input: Test"); - PX4_INFO_RAW(" roll : % .1f deg\n", (double)_roll_deg); - PX4_INFO_RAW(" pitch: % .1f deg\n", (double)_pitch_deg); - PX4_INFO_RAW(" yaw : % .1f deg\n", (double)_yaw_deg); - PX4_INFO_RAW(" rollrate : % .1f deg/s\n", (double)_rollrate_deg_s); - PX4_INFO_RAW(" pitchrate: % .1f deg/s\n", (double)_pitchrate_deg_s); - PX4_INFO_RAW(" yawrate : % .1f deg/s\n", (double)_yawrate_deg_s); + PX4_INFO_RAW(" roll : % .1f deg\n", (double)_roll_deg); + PX4_INFO_RAW(" pitch: % .1f deg\n", (double)_pitch_deg); + PX4_INFO_RAW(" yaw : % .1f deg\n", (double)_yaw_deg); + PX4_INFO_RAW(" rollrate : % .1f deg/s\n", (double)_rollrate_deg_s); + PX4_INFO_RAW(" pitchrate: % .1f deg/s\n", (double)_pitchrate_deg_s); + PX4_INFO_RAW(" yawrate : % .1f deg/s\n", (double)_yawrate_deg_s); } void InputTest::set_test_input_angles(float roll_deg, float pitch_deg, float yaw_deg) diff --git a/src/modules/gimbal/output_mavlink.cpp b/src/modules/gimbal/output_mavlink.cpp index c3ceaab678c0..cac5c510da72 100644 --- a/src/modules/gimbal/output_mavlink.cpp +++ b/src/modules/gimbal/output_mavlink.cpp @@ -38,6 +38,7 @@ #include +#include namespace gimbal { @@ -244,10 +245,314 @@ void OutputMavlinkV2::_publish_gimbal_device_set_attitude() } if (_absolute_angle[2]) { - set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_YAW_LOCK; + set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME; + + } else { + set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME; } _gimbal_device_set_attitude_pub.publish(set_attitude); } -} /* namespace gimbal */ +OutputMavlinkToGimbalManager::OutputMavlinkToGimbalManager(const Parameters ¶meters) + : OutputBase(parameters) +{ +} + +const char *OutputMavlinkToGimbalManager::_control_right_str(ControlRights control_right) +{ + switch (control_right) { + case ControlRights::PRIMARY: + return "PRIMARY"; + break; + + case ControlRights::SECONDARY: + return "SECONDARY"; + break; + + case ControlRights::NONE: + default: + return "NONE"; + } +} + +void OutputMavlinkToGimbalManager::update(const ControlData &control_data, bool new_setpoints, uint8_t &gimbal_device_id) +{ + const hrt_abstime now = hrt_absolute_time(); + + gimbal_device_id = 0; + + // Discovery until a valid manager is detected + _check_for_gimbal_manager_information(); + + if (!_have_valid_manager()) { + if ((now - _last_info_request) >= INFO_REQUEST_PERIOD_US) { + _last_info_request = now; + _request_gimbal_manager_information(); + } + + return; + } + + gimbal_device_id = (_gimbal_device_id != 0) ? _gimbal_device_id : 0; + + // Update control ownership state and process any pending take-control ACK + _check_for_gimbal_manager_status(); + _check_for_take_control_ack(); + + // Only publish a new setpoint when fresh control data is available + if (!new_setpoints) { + return; + } + + // We may publish immediately if PX4 already has control, otherwise try to acquire it + _can_publish_set_attitude = (_control_rights != ControlRights::NONE) || (_acquire_control_for_autopilot(now)); + + if (!_can_publish_set_attitude) { return; } + + // Update the outgoing setpoint and forward it to the external gimbal manager + _set_angle_setpoints(control_data); + _handle_position_update(control_data); + _last_update = now; + + _publish_gimbal_manager_set_attitude(); +} + +void OutputMavlinkToGimbalManager::_publish_gimbal_manager_set_attitude() +{ + external_gimbal_manager_set_attitude_s msg{}; + msg.timestamp = hrt_absolute_time(); + + msg.origin_sysid = (uint8_t)_parameters.mav_sysid; + msg.origin_compid = (uint8_t)_parameters.mav_compid; + + msg.target_system = _manager_sysid; + msg.target_component = _manager_compid; + msg.gimbal_device_id = _gimbal_device_id; + + msg.angular_velocity_x = _angle_velocity[0]; + msg.angular_velocity_y = _angle_velocity[1]; + msg.angular_velocity_z = _angle_velocity[2]; + + msg.q[0] = _q_setpoint[0]; + msg.q[1] = _q_setpoint[1]; + msg.q[2] = _q_setpoint[2]; + msg.q[3] = _q_setpoint[3]; + + if (_absolute_angle[2]) { + msg.flags |= external_gimbal_manager_set_attitude_s::EXTERNAL_GIMBAL_MANAGER_FLAGS_YAW_IN_EARTH_FRAME; + + } else { + msg.flags |= external_gimbal_manager_set_attitude_s::EXTERNAL_GIMBAL_MANAGER_FLAGS_YAW_IN_VEHICLE_FRAME; + } + + _external_gimbal_manager_set_attitude_pub.publish(msg); +} + +void OutputMavlinkToGimbalManager::_request_gimbal_manager_information() +{ + vehicle_command_s cmd{}; + cmd.timestamp = hrt_absolute_time(); + cmd.command = vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE; + + cmd.target_system = 0; + cmd.target_component = 0; + + cmd.source_system = (uint8_t)_parameters.mav_sysid; + cmd.source_component = (uint8_t)_parameters.mav_compid; + + cmd.confirmation = 0; + cmd.from_external = false; + cmd.param1 = static_cast(vehicle_command_s::VEHICLE_CMD_GIMBAL_MANAGER_INFORMATION); + + _vehicle_command_pub.publish(cmd); +} + +void OutputMavlinkToGimbalManager::_check_for_gimbal_manager_information() +{ + external_gimbal_manager_information_s info{}; + + if (_external_gimbal_manager_information_sub.update(&info)) { + _manager_sysid = info.source_system; + _manager_compid = info.source_component; + _gimbal_device_id = info.gimbal_device_id; + } +} + +void OutputMavlinkToGimbalManager::_check_for_gimbal_manager_status() +{ + external_gimbal_manager_status_s gm_st{}; + + if (_external_gimbal_manager_status_sub.update(&gm_st)) { + if (gm_st.gimbal_device_id == 0) { + return; + } + + if (_gimbal_device_id != 0 && gm_st.gimbal_device_id != _gimbal_device_id) { + return; + } + + if (gm_st.source_system == _parameters.mav_sysid + && gm_st.source_component == _parameters.mav_compid) { // Reject GIMBAL_MANAGER_STATUS published by PX4 + return; + } + + const bool have_primary_control = + ((gm_st.primary_control_sysid == _parameters.mav_sysid) && + (gm_st.primary_control_compid == _parameters.mav_compid)); + + const bool have_secondary_control = + ((gm_st.secondary_control_sysid == _parameters.mav_sysid) && + (gm_st.secondary_control_compid == _parameters.mav_compid)); + + if (have_primary_control) { + _control_rights = ControlRights::PRIMARY; + } + + else if (have_secondary_control) { + _control_rights = ControlRights::SECONDARY; + + } else { + _control_rights = ControlRights::NONE; + } + } +} + +void OutputMavlinkToGimbalManager::_reset_take_control_state() +{ + _take_control_retry_count = 0; + _wait_ack_start_time = 0; + _take_control_ack_received = false; + _take_control_ack_accepted = false; +} + +bool OutputMavlinkToGimbalManager::_acquire_control_for_autopilot(const hrt_abstime &now) +{ + const bool gimbal_busy_active = (_take_control_backoff_start != 0) && ((now - _take_control_backoff_start) < GIMBAL_BUSY_TIMEOUT_US); + const bool wait_ack_active = (_wait_ack_start_time != 0) && ((now - _wait_ack_start_time) < WAIT_ACK_TIMEOUT_US); + + if (_control_rights != ControlRights::NONE) { + _reset_take_control_state(); + return true; + } + + if (gimbal_busy_active) { + return false; + } + + if (!wait_ack_active) { + if (_take_control_retry_count < 3) { + _send_take_control_request(); + return true; + } + + events::send(events::ID("gimbal_take_control_limit_reached"), events::Log::Warning, + "Gimbal control could not be acquired, backing off for 15s"); + PX4_WARN("Gimbal control could not be acquired after multiple attempts, backing off for 15s"); + + _take_control_backoff_start = now; + _reset_take_control_state(); + return false; + } + + if (!_take_control_ack_received) { + return true; + } + + if (!_take_control_ack_accepted) { + events::send(events::ID("gimbal_take_control_rejected"), events::Log::Error, + "Gimbal manager rejected PX4 control request, backing off for 15s"); + PX4_ERR("Gimbal manager rejected PX4 take control request, backing off for 15s"); + _take_control_backoff_start = now; + _reset_take_control_state(); + return false; + } + + if (_take_control_retry_count < 3) { + _send_take_control_request(); + return true; + } + + events::send(events::ID("gimbal_take_control_no_rights_limit_reached"), events::Log::Warning, + "Gimbal control could not be acquired, backing off for 15s"); + PX4_WARN("Gimbal control could not be acquired after multiple attempts, backing off for 15s"); + _take_control_backoff_start = now; + _reset_take_control_state(); + return false; +} + +bool OutputMavlinkToGimbalManager::_check_for_take_control_ack() +{ + vehicle_command_ack_s ack{}; + + if (!_vehicle_command_ack_sub.update(&ack)) { + return false; + } + + if (ack.command != vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE) { + return false; + } + + _take_control_ack_received = true; + _take_control_ack_accepted = _handle_take_control_command_ack(ack); + return true; +} + +bool OutputMavlinkToGimbalManager::_handle_take_control_command_ack(const vehicle_command_ack_s &ack) +{ + if (ack.result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED + || ack.result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS) { + return true; + } + + return false; +} + +void OutputMavlinkToGimbalManager::_send_take_control_request() +{ + vehicle_command_s cmd{}; + cmd.timestamp = hrt_absolute_time(); + cmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + + cmd.target_system = _manager_sysid; + cmd.target_component = _manager_compid; + + cmd.source_system = static_cast(_parameters.mav_sysid); + cmd.source_component = static_cast(_parameters.mav_compid); + + cmd.confirmation = 0; + cmd.from_external = false; + + cmd.param1 = -2.0f; + cmd.param2 = -2.0f; + + cmd.param3 = -1.f; + cmd.param4 = -1.f; + cmd.param7 = static_cast(_gimbal_device_id); + + _vehicle_command_pub.publish(cmd); + + PX4_INFO("SEND TAKE CTRL to %u/%u from %u/%u", + cmd.target_system, cmd.target_component, + cmd.source_system, cmd.source_component); + + _take_control_retry_count++; + _wait_ack_start_time = cmd.timestamp; + _take_control_ack_received = false; + _take_control_ack_accepted = false; +} + +void OutputMavlinkToGimbalManager::print_status() const +{ + PX4_INFO("Output: MAVLink -> GimbalManager"); + + PX4_INFO_RAW("\n\n"); + + PX4_INFO(" device id: %u", (unsigned)_gimbal_device_id); + PX4_INFO(" our sys/comp: %u/%u", (unsigned)_parameters.mav_sysid, (unsigned)_parameters.mav_compid); + PX4_INFO(" gimbal manager sys/comp: %u/%u", (unsigned)_manager_sysid, (unsigned)_manager_compid); + PX4_INFO(" have valid manager: %s", _have_valid_manager() ? "true" : "false"); + PX4_INFO(" control_rights: %s", _control_right_str(_control_rights)); +} +} + diff --git a/src/modules/gimbal/output_mavlink.h b/src/modules/gimbal/output_mavlink.h index 0e9d9f217fec..5cdbd79dbf7d 100644 --- a/src/modules/gimbal/output_mavlink.h +++ b/src/modules/gimbal/output_mavlink.h @@ -37,10 +37,23 @@ #include "output.h" #include +#include + #include +#include +#include #include #include #include +#include +#include +#include +#include +#include +#include + +#include +#include namespace gimbal @@ -86,4 +99,85 @@ class OutputMavlinkV2 : public OutputBase bool _gimbal_device_found {false}; }; +/** + * @brief Bridges PX4 gimbal setpoints to an external MAVLink gimbal manager (Gremsy Lynx) + * + * @class OutputMavlinkToGimbalManager + */ +class OutputMavlinkToGimbalManager : public OutputBase +{ +public: + explicit OutputMavlinkToGimbalManager(const Parameters ¶meters); + ~OutputMavlinkToGimbalManager() override = default; + + void update(const ControlData &control_data, bool new_setpoints, uint8_t &gimbal_device_id) override; + void print_status() const override; + +private: + // uORB publications + uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; + uORB::Publication _external_gimbal_manager_set_attitude_pub{ORB_ID(external_gimbal_manager_set_attitude)}; + + // uORB subscriptions + uORB::Subscription _external_gimbal_manager_information_sub{ORB_ID(external_gimbal_manager_information)}; + uORB::Subscription _external_gimbal_manager_status_sub{ORB_ID(external_gimbal_manager_status)}; + uORB::Subscription _vehicle_command_ack_sub{ORB_ID(vehicle_command_ack)}; + + /** + * Control ownership reported by the external gimbal manager + */ + enum class ControlRights : uint8_t { + NONE = 0, + PRIMARY = 1, + SECONDARY = 2 + }; + + // Discovery + void _check_for_gimbal_manager_information(); + void _request_gimbal_manager_information(); + void _check_for_gimbal_manager_status(); + + // Control ownership + bool _check_for_take_control_ack(); + bool _handle_take_control_command_ack(const vehicle_command_ack_s &ack); + + bool _acquire_control_for_autopilot(const hrt_abstime &now); + void _send_take_control_request(); + void _reset_take_control_state(); + + // Setpoint output + void _publish_gimbal_manager_set_attitude(); + + // Helpers + bool _have_valid_manager() const + { + return ((_manager_sysid != 0) && + (_manager_compid != 0) && + (_gimbal_device_id != 0)); + } + + static const char *_control_right_str(ControlRights control_rights); + + // Cache identity of the external gimbal manager + uint8_t _manager_sysid{0}; + uint8_t _manager_compid{0}; + uint8_t _gimbal_device_id{0}; + + // Control state + ControlRights _control_rights{ControlRights::NONE}; + uint8_t _take_control_retry_count{0}; + + bool _can_publish_set_attitude{false}; + bool _take_control_ack_received{false}; + bool _take_control_ack_accepted{false}; + + // Timing state + hrt_abstime _last_info_request{0}; + hrt_abstime _wait_ack_start_time{0}; + hrt_abstime _take_control_backoff_start{0}; + + static constexpr hrt_abstime INFO_REQUEST_PERIOD_US = 1'000'000; ///< Discovery request rate + static constexpr hrt_abstime GIMBAL_BUSY_TIMEOUT_US = 15'000'000; ///< Backoff after repeated failures + static constexpr hrt_abstime WAIT_ACK_TIMEOUT_US = 1'000'000; ///< Timeout while waiting for ACK +}; } /* namespace gimbal */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index d7bda2b63d08..e8744bcc8d0a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -110,6 +110,18 @@ static bool accept_unsigned_callback(const mavlink_status_t *status, uint32_t me return false; } +static int32_t get_mnt_mode_out() +{ + param_t param_handle = param_find_no_notification("MNT_MODE_OUT"); + int32_t mnt_mode_out = -1; // Mount mode out + + if (param_handle != PARAM_INVALID) { + param_get(param_handle, &mnt_mode_out); + } + + return mnt_mode_out; +} + static void usage(); hrt_abstime Mavlink::_first_start_time = {0}; @@ -1501,10 +1513,18 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) #endif configure_stream_local("EXTENDED_SYS_STATE", 1.0f); configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 5.0f); - configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f); - configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f); + + if (get_mnt_mode_out() == static_cast(gimbal::MNT_MODE_OUT_TO_GIMBAL_MANAGER)) { + configure_stream_local("EXTERNAL_GIMBAL_MANAGER_SET_ATTITUDE", 5.0f); + + } else { + configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f); + configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f); + } + configure_stream_local("GLOBAL_POSITION_SENSOR", 5.0f); configure_stream_local("GLOBAL_POSITION_INT", 5.0f); + #if defined(MAVLINK_MSG_ID_GNSS_INTEGRITY) configure_stream_local("GNSS_INTEGRITY", 1.0f); #endif @@ -1583,8 +1603,15 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) #endif configure_stream_local("EXTENDED_SYS_STATE", 5.0f); configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 5.0f); - configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f); - configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f); + + if (get_mnt_mode_out() == static_cast(gimbal::MNT_MODE_OUT_TO_GIMBAL_MANAGER)) { + configure_stream_local("EXTERNAL_GIMBAL_MANAGER_SET_ATTITUDE", 5.0f); + + } else { + configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f); + configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f); + } + configure_stream_local("GLOBAL_POSITION_INT", 50.0f); #if defined(MAVLINK_MSG_ID_GNSS_INTEGRITY) configure_stream_local("GNSS_INTEGRITY", 1.0f); @@ -1634,7 +1661,14 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) case MAVLINK_MODE_GIMBAL: // Note: streams requiring low latency come first configure_stream_local("AUTOPILOT_STATE_FOR_GIMBAL_DEVICE", 20.0f); - configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 20.0f); + + if (get_mnt_mode_out() == static_cast(gimbal::MNT_MODE_OUT_TO_GIMBAL_MANAGER)) { + configure_stream_local("EXTERNAL_GIMBAL_MANAGER_SET_ATTITUDE", 20.0f); + + } else { + configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 20.0f); + } + break; case MAVLINK_MODE_EXTVISION: @@ -1843,8 +1877,15 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("MOUNT_ORIENTATION", 10.0f); configure_stream_local("OBSTACLE_DISTANCE", 10.0f); configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 5.0f); - configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f); - configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f); + + if (get_mnt_mode_out() == static_cast(gimbal::MNT_MODE_OUT_TO_GIMBAL_MANAGER)) { + configure_stream_local("EXTERNAL_GIMBAL_MANAGER_SET_ATTITUDE", 5.0f); + + } else { + configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f); + configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f); + } + configure_stream_local("ESC_INFO", 1.0f); configure_stream_local("ESC_STATUS", 5.0f); #if defined(MAVLINK_MSG_ID_ESC_EEPROM) @@ -1912,8 +1953,15 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("MOUNT_ORIENTATION", 2.0f); configure_stream_local("OBSTACLE_DISTANCE", 2.0f); configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 5.0f); - configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f); - configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 2.0f); + + if (get_mnt_mode_out() == static_cast(gimbal::MNT_MODE_OUT_TO_GIMBAL_MANAGER)) { + configure_stream_local("EXTERNAL_GIMBAL_MANAGER_SET_ATTITUDE", 2.0f); + + } else { + configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 2.0f); + configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f); + } + configure_stream_local("ESC_INFO", 1.0f); configure_stream_local("ESC_STATUS", 2.0f); #if defined(MAVLINK_MSG_ID_ESC_EEPROM) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index fde7524ff322..d6b06d26bd36 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -87,6 +87,8 @@ #include "mavlink_shell.h" #include "mavlink_ulog.h" +#include "gimbal/gimbal_params.h" + #define DEFAULT_BAUD_RATE 57600 #define DEFAULT_DEVICE_NAME "/dev/ttyS1" @@ -496,7 +498,6 @@ class Mavlink final : public ModuleParams void set_signing_key_dirty() { _signing_key_dirty.store(true); } void check_signing_key_dirty() { if (_signing_key_dirty.load()) { _signing_key_dirty.store(false); _sign_control.reload_key(); } } - private: MavlinkReceiver _receiver; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 8eb1f42eea43..fd9ea193b9f5 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -153,6 +153,7 @@ # include "streams/GIMBAL_DEVICE_SET_ATTITUDE.hpp" # include "streams/GIMBAL_MANAGER_INFORMATION.hpp" # include "streams/GIMBAL_MANAGER_STATUS.hpp" +# include "streams/EXTERNAL_GIMBAL_MANAGER_SET_ATTITUDE.hpp" # include "streams/GIMBAL_DEVICE_INFORMATION.hpp" # include "streams/GPS2_RAW.hpp" # include "streams/HIGH_LATENCY2.hpp" @@ -364,6 +365,9 @@ static const StreamListItem streams_list[] = { #if defined(GIMBAL_MANAGER_STATUS_HPP) create_stream_list_item(), #endif // GIMBAL_MANAGER_STATUS_HPP +#if defined(EXTERNAL_GIMBAL_MANAGER_SET_ATTITUDE_HPP) + create_stream_list_item(), +#endif // EXTERNAL_GIMBAL_MANAGER_SET_ATTITUDE #if defined(GIMBAL_DEVICE_SET_ATTITUDE_HPP) create_stream_list_item(), #endif // GIMBAL_DEVICE_SET_ATTITUDE_HPP diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 60507e533f57..825fe0428930 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -323,6 +323,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_gimbal_manager_set_manual_control(msg); break; + case MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION: + handle_message_external_gimbal_manager_information(msg); + break; + + case MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS: + handle_message_external_gimbal_manager_status(msg); + break; + case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION: handle_message_gimbal_device_information(msg); break; @@ -441,6 +449,14 @@ void MavlinkReceiver::handle_messages_in_gimbal_mode(mavlink_message_t &msg) handle_message_gimbal_manager_set_manual_control(&msg); break; + case MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION: + handle_message_external_gimbal_manager_information(&msg); + break; + + case MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS: + handle_message_external_gimbal_manager_status(&msg); + break; + case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION: handle_message_gimbal_device_information(&msg); break; @@ -564,6 +580,54 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) handle_message_command_both(msg, cmd_mavlink, vcmd); } +void MavlinkReceiver::handle_message_external_gimbal_manager_information(mavlink_message_t *msg) +{ + mavlink_gimbal_manager_information_t m{}; + mavlink_msg_gimbal_manager_information_decode(msg, &m); + + external_gimbal_manager_information_s info{}; + info.timestamp = hrt_absolute_time(); + + // Sender identity from MAVLink header + info.source_system = msg -> sysid; + info.source_component = msg -> compid; + + // Payload fields from decoded MAVLink message + info.cap_flags = m.cap_flags; + info.gimbal_device_id = m.gimbal_device_id; + info.roll_min = m.roll_min; + info.roll_max = m.roll_max; + info.pitch_min = m.pitch_min; + info.pitch_max = m.pitch_max; + info.yaw_min = m.yaw_min; + info.yaw_max = m.yaw_max; + + _external_gimbal_manager_information_pub.publish(info); +} + +void MavlinkReceiver::handle_message_external_gimbal_manager_status(mavlink_message_t *msg) +{ + mavlink_gimbal_manager_status_t m{}; + mavlink_msg_gimbal_manager_status_decode(msg, &m); + + external_gimbal_manager_status_s st{}; + st.timestamp = hrt_absolute_time(); + + st.flags = m.flags; + st.gimbal_device_id = m.gimbal_device_id; + + st.primary_control_sysid = m.primary_control_sysid; + st.primary_control_compid = m.primary_control_compid; + + st.secondary_control_sysid = m.secondary_control_sysid; + st.secondary_control_compid = m.secondary_control_compid; + + st.source_system = msg -> sysid; + st.source_component = msg -> compid; + + _external_gimbal_manager_status_pub.publish(st); +} + template void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink, const vehicle_command_s &vehicle_command) diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index e2adf224dbc1..6c1c96f25791 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -73,6 +73,8 @@ #include #include #include +#include +#include #include #include #include @@ -215,6 +217,8 @@ class MavlinkReceiver : public ModuleParams void handle_message_vision_position_estimate(mavlink_message_t *msg); void handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg); void handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg); + void handle_message_external_gimbal_manager_information(mavlink_message_t *msg); + void handle_message_external_gimbal_manager_status(mavlink_message_t *msg); void handle_message_gimbal_device_information(mavlink_message_t *msg); void handle_message_gimbal_device_attitude_status(mavlink_message_t *msg); void handle_message_global_position_sensor(mavlink_message_t *msg); @@ -315,6 +319,8 @@ class MavlinkReceiver : public ModuleParams uORB::Publication _camera_status_pub{ORB_ID(camera_status)}; uORB::Publication _cellular_status_pub{ORB_ID(cellular_status)}; uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; + uORB::Publication _external_gimbal_manager_information_pub{ORB_ID(external_gimbal_manager_information)}; + uORB::Publication _external_gimbal_manager_status_pub{ORB_ID(external_gimbal_manager_status)}; uORB::Publication _follow_target_pub{ORB_ID(follow_target)}; uORB::Publication _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)}; uORB::Publication _gimbal_manager_set_manual_control_pub{ORB_ID(gimbal_manager_set_manual_control)}; diff --git a/src/modules/mavlink/streams/EXTERNAL_GIMBAL_MANAGER_SET_ATTITUDE.hpp b/src/modules/mavlink/streams/EXTERNAL_GIMBAL_MANAGER_SET_ATTITUDE.hpp new file mode 100644 index 000000000000..5994125d5444 --- /dev/null +++ b/src/modules/mavlink/streams/EXTERNAL_GIMBAL_MANAGER_SET_ATTITUDE.hpp @@ -0,0 +1,61 @@ +#ifndef EXTERNAL_GIMBAL_MANAGER_SET_ATTITUDE_HPP +#define EXTERNAL_GIMBAL_MANAGER_SET_ATTITUDE_HPP + +#include + +class MavlinkStreamExternalGimbalManagerSetAttitude : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamExternalGimbalManagerSetAttitude(mavlink); } + + static constexpr const char *get_name_static() { return "EXTERNAL_GIMBAL_MANAGER_SET_ATTITUDE"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override { return _external_gimbal_manager_set_attitude_sub.advertised() ? MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } + +private: + explicit MavlinkStreamExternalGimbalManagerSetAttitude(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _external_gimbal_manager_set_attitude_sub{ORB_ID(external_gimbal_manager_set_attitude)}; + + bool send() override + { + external_gimbal_manager_set_attitude_s sp{}; + + if (_external_gimbal_manager_set_attitude_sub.update(&sp)) { + + //Avoid sending commands addressed to ourselves (common when this topic is used as input) + if ((sp.target_system == mavlink_system.sysid) && + (sp.target_component == mavlink_system.compid)) { + return false; + } + + mavlink_gimbal_manager_set_attitude_t msg{}; + msg.target_system = sp.target_system; + msg.target_component = sp.target_component; + + msg.flags = sp.flags; + msg.gimbal_device_id = sp.gimbal_device_id; + + msg.q[0] = sp.q[0]; + msg.q[1] = sp.q[1]; + msg.q[2] = sp.q[2]; + msg.q[3] = sp.q[3]; + + msg.angular_velocity_x = sp.angular_velocity_x; + msg.angular_velocity_y = sp.angular_velocity_y; + msg.angular_velocity_z = sp.angular_velocity_z; + + mavlink_msg_gimbal_manager_set_attitude_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif //EXTERNAL_GIMBAL_MANAGER_SET_ATTITUDE_HPP From effda0252f92b1309d6bfba0deac52589f477b56 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C4=90=E1=BB=97=20B=E1=BA=A3o=20Long?= Date: Tue, 21 Apr 2026 07:50:57 +0000 Subject: [PATCH 2/3] [Fix] Cannot build for zenoh board --- boards/auterion/fmu-v6x/zenoh.px4board | 3 +++ src/modules/gimbal/output_mavlink.cpp | 1 - 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/boards/auterion/fmu-v6x/zenoh.px4board b/boards/auterion/fmu-v6x/zenoh.px4board index 682743078640..885ace83fc80 100644 --- a/boards/auterion/fmu-v6x/zenoh.px4board +++ b/boards/auterion/fmu-v6x/zenoh.px4board @@ -3,4 +3,7 @@ CONFIG_DRIVERS_UAVCAN=n CONFIG_MODULES_UXRCE_DDS_CLIENT=n CONFIG_SYSTEMCMDS_SD_BENCH=n CONFIG_SYSTEMCMDS_I2CDETECT=n +CONFIG_MODULES_GIMBAL=n +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n +CONFIG_MODULES_ROVER_MECANUM=n CONFIG_MODULES_ZENOH=y diff --git a/src/modules/gimbal/output_mavlink.cpp b/src/modules/gimbal/output_mavlink.cpp index cac5c510da72..5d6572451baf 100644 --- a/src/modules/gimbal/output_mavlink.cpp +++ b/src/modules/gimbal/output_mavlink.cpp @@ -555,4 +555,3 @@ void OutputMavlinkToGimbalManager::print_status() const PX4_INFO(" control_rights: %s", _control_right_str(_control_rights)); } } - From 6611087c34b4145b95e1e043b33658de74c6ea5e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C4=90=E1=BB=97=20B=E1=BA=A3o=20Long?= Date: Tue, 21 Apr 2026 10:05:44 +0000 Subject: [PATCH 3/3] Disable unnecessary to avoid overflow memory issue --- boards/auterion/fmu-v6x/default.px4board | 2 +- src/modules/gimbal/gimbal_params.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/boards/auterion/fmu-v6x/default.px4board b/boards/auterion/fmu-v6x/default.px4board index 1cef7804abd5..770e1d6423f0 100644 --- a/boards/auterion/fmu-v6x/default.px4board +++ b/boards/auterion/fmu-v6x/default.px4board @@ -77,7 +77,7 @@ CONFIG_NUM_MISSION_ITMES_SUPPORTED=1000 CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_MECANUM=y CONFIG_MODULES_SENSORS=y -CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n CONFIG_MODULES_UXRCE_DDS_CLIENT=y CONFIG_MODULES_VTOL_ATT_CONTROL=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y diff --git a/src/modules/gimbal/gimbal_params.yaml b/src/modules/gimbal/gimbal_params.yaml index bdf2d7fa08ae..e61997a2c616 100644 --- a/src/modules/gimbal/gimbal_params.yaml +++ b/src/modules/gimbal/gimbal_params.yaml @@ -34,7 +34,7 @@ parameters: 0: AUX 1: MAVLink gimbal protocol v1 2: MAVLink gimbal protocol v2 - 3: MAVLink gimbal protocol v3 (mount gimbal manager) + 3: MAVLink gimbal protocol v2 (mount gimbal manager) default: 0 min: 0 max: 3