From 492f14ea05736dcbb55dcb4483675933cefd629d Mon Sep 17 00:00:00 2001 From: Karen Simeon Date: Tue, 10 Feb 2026 12:17:59 +1100 Subject: [PATCH 1/6] AP_Mount_MAVLink: forward MAV_CMD_DO_SET_ROI_LOCATION to gimbal --- libraries/AP_Mount/AP_Mount_Backend.cpp | 25 +++++++++++++++++++-- libraries/AP_Mount/AP_Mount_Backend.h | 2 ++ libraries/AP_Mount/AP_Mount_MAVLink.cpp | 29 +++++++++++++++++++++++++ libraries/AP_Mount/AP_Mount_MAVLink.h | 17 ++++++++++----- libraries/AP_Mount/AP_Mount_Servo.cpp | 1 + 5 files changed, 67 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 72dc05f7a5bcf..0ed7f943b7efc 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -235,6 +235,10 @@ void AP_Mount_Backend::set_roi_target(const Location &target_loc) // set the mode to GPS tracking mode set_mode(MAV_MOUNT_MODE_GPS_POINT); + if (natively_supports(MountTargetType::LOCATION)) { + send_target_location(_roi_target); + } + // optionally set RC_TARGETING yaw lock state if (option_set(Options::RCTARGETING_LOCK_FROM_PREVMODE)) { set_yaw_lock(true); @@ -327,6 +331,10 @@ void AP_Mount_Backend::clear_roi_target() MAV_MOUNT_MODE default_mode = (MAV_MOUNT_MODE)_params.default_mode.get(); set_mode(default_mode); } + + if (natively_supports(MountTargetType::LOCATION)) { + send_target_location(_roi_target); + } } // set_sys_target - sets system that mount should attempt to point towards @@ -942,6 +950,9 @@ uint16_t AP_Mount_Backend::get_gimbal_device_flags() const case MountTargetType::NEUTRAL: yaw_lock_state = false; // not locked onto the scenery break; + case MountTargetType::LOCATION: + yaw_lock_state = true; + break; } break; case MAV_MOUNT_MODE_RC_TARGETING: @@ -1045,8 +1056,8 @@ void AP_Mount_Backend::_update_mnt_target() case MAV_MOUNT_MODE_GPS_POINT: // point mount to a GPS point given by the mission planner - if (get_angle_target_to_roi(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; + if (AP::ahrs().get_location(_roi_target)) { + mnt_target.target_type = MountTargetType::LOCATION; } return; @@ -1087,6 +1098,8 @@ void AP_Mount_Backend::send_target_to_gimbal() case MountTargetType::NEUTRAL: send_target_neutral(); return; + case MountTargetType::LOCATION: + return; } return; // should not reach this as all cases return } @@ -1127,6 +1140,14 @@ void AP_Mount_Backend::send_target_to_gimbal() return; } break; + case MountTargetType::LOCATION: + if (natively_supports(MountTargetType::ANGLE)) { + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + send_target_angles(mnt_target.angle_rad); + } + return; + } + break; } send_warning_to_GCS("Failed to convert mount target to command gimbal"); diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index bfcd8d96cd9c3..caaf3052bda34 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -243,6 +243,7 @@ class AP_Mount_Backend RATE = 1, RETRACTED = 2, NEUTRAL = 3, + LOCATION = 4, }; // class for a single angle or rate target @@ -306,6 +307,7 @@ class AP_Mount_Backend virtual void send_target_rates(const MountRateTarget &rate_rads) { } virtual void send_target_retracted() { } virtual void send_target_neutral() { } + virtual void send_target_location(const Location &roi_loc) { } // options parameter bitmask handling enum class Options : uint8_t { diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_MAVLink.cpp index 909979eae082b..16cc40c9291da 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.cpp +++ b/libraries/AP_Mount/AP_Mount_MAVLink.cpp @@ -136,6 +136,8 @@ void AP_Mount_MAVLink::handle_gimbal_device_information(const mavlink_message_t const uint8_t fw_ver_revision = (info.firmware_version & 0x00FF0000) >> 16; const uint8_t fw_ver_build = (info.firmware_version & 0xFF000000) >> 24; + strcpy(_vendor_name, info.vendor_name); + // display gimbal info to user GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mount: %s %s fw:%u.%u.%u.%u", info.vendor_name, @@ -277,4 +279,31 @@ void AP_Mount_MAVLink::send_target_angles(const MountAngleTarget &angle_rad) _link->send_message(MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, (const char*)&pkt); } +// Send MAV_CMD_DO_SET_ROI_LOCATION to gimbal +void AP_Mount_MAVLink::send_target_location(const Location &roi_loc) +{ + if (_link == nullptr) { + return; + } + + mavlink_command_int_t pkt; + pkt.target_system = _sysid; + pkt.target_component = _compid; + + if (roi_loc.initialised()) + { + pkt.command = MAV_CMD_DO_SET_ROI_LOCATION; + pkt.x = roi_loc.lat, // param5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + pkt.y = roi_loc.lng, // param6 / local: y position in meters * 1e4, global: longitude in degrees * 10^7 + pkt.z = roi_loc.alt * 0.01f; // param7 / z position: global: altitude in meters (relative or absolute, depending on frame). + pkt.frame = (uint8_t)roi_loc.get_alt_frame(); + } + else + { + pkt.command = MAV_CMD_DO_SET_ROI_NONE; + } + + _link->send_message(MAVLINK_MSG_ID_COMMAND_INT, (const char*)&pkt); +} + #endif // HAL_MOUNT_MAVLINK_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.h b/libraries/AP_Mount/AP_Mount_MAVLink.h index c9d636db41088..3fc0ec53e820b 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.h +++ b/libraries/AP_Mount/AP_Mount_MAVLink.h @@ -40,11 +40,15 @@ class AP_Mount_MAVLink : public AP_Mount_Backend // MVAVLink can send either rates or angles, and can also be // directly commanded to move to a retracted state uint8_t natively_supported_mount_target_types() const override { - return ( - (1U< Date: Wed, 11 Feb 2026 09:24:14 +1100 Subject: [PATCH 2/6] AP_Mount_Backend: move send_target_location for set_roi to send_target_gimbal --- libraries/AP_Mount/AP_Mount_Backend.cpp | 9 ++------- libraries/AP_Mount/AP_Mount_MAVLink.cpp | 11 ++++------- libraries/AP_Mount/AP_Mount_MAVLink.h | 15 +++++++++------ 3 files changed, 15 insertions(+), 20 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 0ed7f943b7efc..66c187d2fe633 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -235,10 +235,6 @@ void AP_Mount_Backend::set_roi_target(const Location &target_loc) // set the mode to GPS tracking mode set_mode(MAV_MOUNT_MODE_GPS_POINT); - if (natively_supports(MountTargetType::LOCATION)) { - send_target_location(_roi_target); - } - // optionally set RC_TARGETING yaw lock state if (option_set(Options::RCTARGETING_LOCK_FROM_PREVMODE)) { set_yaw_lock(true); @@ -1056,9 +1052,7 @@ void AP_Mount_Backend::_update_mnt_target() case MAV_MOUNT_MODE_GPS_POINT: // point mount to a GPS point given by the mission planner - if (AP::ahrs().get_location(_roi_target)) { - mnt_target.target_type = MountTargetType::LOCATION; - } + mnt_target.target_type = MountTargetType::LOCATION; return; case MAV_MOUNT_MODE_HOME_LOCATION: @@ -1099,6 +1093,7 @@ void AP_Mount_Backend::send_target_to_gimbal() send_target_neutral(); return; case MountTargetType::LOCATION: + send_target_location(_roi_target); return; } return; // should not reach this as all cases return diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_MAVLink.cpp index 16cc40c9291da..8920595037c96 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.cpp +++ b/libraries/AP_Mount/AP_Mount_MAVLink.cpp @@ -136,7 +136,7 @@ void AP_Mount_MAVLink::handle_gimbal_device_information(const mavlink_message_t const uint8_t fw_ver_revision = (info.firmware_version & 0x00FF0000) >> 16; const uint8_t fw_ver_build = (info.firmware_version & 0xFF000000) >> 24; - strcpy(_vendor_name, info.vendor_name); + strncpy(vendor_name, info.vendor_name, ARRAY_SIZE(vendor_name)); // display gimbal info to user GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mount: %s %s fw:%u.%u.%u.%u", @@ -286,20 +286,17 @@ void AP_Mount_MAVLink::send_target_location(const Location &roi_loc) return; } - mavlink_command_int_t pkt; + mavlink_command_int_t pkt {}; pkt.target_system = _sysid; pkt.target_component = _compid; - if (roi_loc.initialised()) - { + if (roi_loc.initialised()) { pkt.command = MAV_CMD_DO_SET_ROI_LOCATION; pkt.x = roi_loc.lat, // param5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 pkt.y = roi_loc.lng, // param6 / local: y position in meters * 1e4, global: longitude in degrees * 10^7 pkt.z = roi_loc.alt * 0.01f; // param7 / z position: global: altitude in meters (relative or absolute, depending on frame). pkt.frame = (uint8_t)roi_loc.get_alt_frame(); - } - else - { + } else { pkt.command = MAV_CMD_DO_SET_ROI_NONE; } diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.h b/libraries/AP_Mount/AP_Mount_MAVLink.h index 3fc0ec53e820b..519cb6cbffe0b 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.h +++ b/libraries/AP_Mount/AP_Mount_MAVLink.h @@ -41,11 +41,14 @@ class AP_Mount_MAVLink : public AP_Mount_Backend // directly commanded to move to a retracted state uint8_t natively_supported_mount_target_types() const override { - uint8_t supported_target = (1U< Date: Wed, 11 Feb 2026 09:32:03 +1100 Subject: [PATCH 3/6] AP_Mount_MAVLink: use constant for vendor name length --- libraries/AP_Mount/AP_Mount_MAVLink.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.h b/libraries/AP_Mount/AP_Mount_MAVLink.h index 519cb6cbffe0b..c304976989f12 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.h +++ b/libraries/AP_Mount/AP_Mount_MAVLink.h @@ -89,6 +89,6 @@ class AP_Mount_MAVLink : public AP_Mount_Backend uint8_t _compid; // component id of gimbal mavlink_gimbal_device_attitude_status_t _gimbal_device_attitude_status; // copy of most recently received gimbal status uint32_t _last_attitude_status_ms; // system time last attitude status was received (used for health reporting) - char vendor_name[32]; // vendor name + char vendor_name[MAVLINK_MSG_GIMBAL_DEVICE_INFORMATION_FIELD_VENDOR_NAME_LEN]; // vendor name }; #endif // HAL_MOUNT_MAVLINK_ENABLED From 1aabc911d49fda174fa4f343b98dc692264c0ab2 Mon Sep 17 00:00:00 2001 From: Karen Simeon Date: Thu, 12 Feb 2026 12:04:18 +1100 Subject: [PATCH 4/6] AP_Mount_Backend: process clear roi in send_target_to_gimbal --- libraries/AP_Mount/AP_Mount_Backend.cpp | 11 ++++++++--- libraries/AP_Mount/AP_Mount_Backend.h | 3 ++- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 66c187d2fe633..42a93071110d1 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -328,9 +328,7 @@ void AP_Mount_Backend::clear_roi_target() set_mode(default_mode); } - if (natively_supports(MountTargetType::LOCATION)) { - send_target_location(_roi_target); - } + clear_roi_pending = true; } // set_sys_target - sets system that mount should attempt to point towards @@ -1077,6 +1075,13 @@ void AP_Mount_Backend::_update_mnt_target() void AP_Mount_Backend::send_target_to_gimbal() { + // process any pending clear-roi-target + // it is assumed that we have already zeroed _roi_target + if (clear_roi_pending && natively_supports(MountTargetType::LOCATION)) { + clear_roi_pending = false; + send_target_location(_roi_target); + } + // the easy case, where the gimbal natively supports the MntTargetType: if (natively_supports(mnt_target.target_type)) { switch (mnt_target.target_type) { diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index caaf3052bda34..adda646a76815 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -400,7 +400,8 @@ class AP_Mount_Backend // RP earth frame locks accessible by backend bool _pitch_lock = true; // pitch_lock used in RC_TARGETING mode. True if the gimbal's tilt target is maintained in earth-frame, if false (aka "follow") it is maintained in body-frame bool _roll_lock = true; // roll_lock used in RC_TARGETING mode. True if the gimbal's roll target is maintained in earth-frame, if false (aka "follow") it is maintained in body-frame - + + bool clear_roi_pending = false; // True if there is a pending clear ROI request private: From c7567707d207dd8d51e1ed23eab0ce5d7cc319db Mon Sep 17 00:00:00 2001 From: Karen Simeon Date: Mon, 16 Feb 2026 10:50:09 +1100 Subject: [PATCH 5/6] AP_Mount_MAVLink: use vendor and model name to determine if location target is supported --- libraries/AP_Mount/AP_Mount_Backend.h | 2 +- libraries/AP_Mount/AP_Mount_MAVLink.cpp | 1 + libraries/AP_Mount/AP_Mount_MAVLink.h | 4 +++- 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index adda646a76815..90abb08d494cd 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -401,7 +401,7 @@ class AP_Mount_Backend bool _pitch_lock = true; // pitch_lock used in RC_TARGETING mode. True if the gimbal's tilt target is maintained in earth-frame, if false (aka "follow") it is maintained in body-frame bool _roll_lock = true; // roll_lock used in RC_TARGETING mode. True if the gimbal's roll target is maintained in earth-frame, if false (aka "follow") it is maintained in body-frame - bool clear_roi_pending = false; // True if there is a pending clear ROI request + bool clear_roi_pending; // True if there is a pending clear ROI request private: diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_MAVLink.cpp index 8920595037c96..618bb7ffcfc4b 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.cpp +++ b/libraries/AP_Mount/AP_Mount_MAVLink.cpp @@ -137,6 +137,7 @@ void AP_Mount_MAVLink::handle_gimbal_device_information(const mavlink_message_t const uint8_t fw_ver_build = (info.firmware_version & 0xFF000000) >> 24; strncpy(vendor_name, info.vendor_name, ARRAY_SIZE(vendor_name)); + strncpy(model_name, info.model_name, ARRAY_SIZE(model_name)); // display gimbal info to user GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mount: %s %s fw:%u.%u.%u.%u", diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.h b/libraries/AP_Mount/AP_Mount_MAVLink.h index c304976989f12..f57aee7474354 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.h +++ b/libraries/AP_Mount/AP_Mount_MAVLink.h @@ -48,8 +48,9 @@ class AP_Mount_MAVLink : public AP_Mount_Backend ); // temporary hack until we get GIMBAL_DEVICE_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL - if (strncmp(vendor_name, "AVTA", 4) == 0) + if ((strncmp(vendor_name, "AVTA", 4) == 0) && (strncmp(model_name, "CM41", 4) != 0)){ supported_target |= (1U< Date: Tue, 17 Feb 2026 15:39:11 +1100 Subject: [PATCH 6/6] AP_Mount_MAVLink: Add function description for send_target_location --- libraries/AP_Mount/AP_Mount_MAVLink.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.h b/libraries/AP_Mount/AP_Mount_MAVLink.h index f57aee7474354..f300d3f827484 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.h +++ b/libraries/AP_Mount/AP_Mount_MAVLink.h @@ -79,6 +79,7 @@ class AP_Mount_MAVLink : public AP_Mount_Backend // send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to control attitude void send_target_angles(const MountAngleTarget &angle_rad) override; + // Send MAV_CMD_DO_SET_ROI to gimbal to point at a location void send_target_location(const Location &roi_loc) override; // internal variables