Skip to content

AP_Mount_MAVLink: forward MAV_CMD_DO_SET_ROI_LOCATION to gimbal #32162

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
peterbarker merged 6 commits into from
Feb 20, 2026
Merged

AP_Mount_MAVLink: forward MAV_CMD_DO_SET_ROI_LOCATION to gimbal #32162

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
492f14e
AP_Mount_MAVLink: forward MAV_CMD_DO_SET_ROI_LOCATION to gimbal
KSimeonAVTA Feb 10, 2026
67f414c
AP_Mount_Backend: move send_target_location for set_roi to send_targe…
KSimeonAVTA Feb 10, 2026
5d9d2a7
AP_Mount_MAVLink: use constant for vendor name length
KSimeonAVTA Feb 10, 2026
1aabc91
AP_Mount_Backend: process clear roi in send_target_to_gimbal
KSimeonAVTA Feb 12, 2026
c756770
AP_Mount_MAVLink: use vendor and model name to determine if location …
KSimeonAVTA Feb 15, 2026
aeeb3cc
AP_Mount_MAVLink: Add function description for send_target_location
KSimeonAVTA Feb 17, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 24 additions & 3 deletions libraries/AP_Mount/AP_Mount_Backend.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -327,6 +327,8 @@ void AP_Mount_Backend::clear_roi_target()
MAV_MOUNT_MODE default_mode = (MAV_MOUNT_MODE)_params.default_mode.get();
set_mode(default_mode);
}

clear_roi_pending = true;
}

// set_sys_target - sets system that mount should attempt to point towards
Expand Down Expand Up @@ -942,6 +944,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:
Expand Down Expand Up @@ -1045,9 +1050,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 (get_angle_target_to_roi(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
mnt_target.target_type = MountTargetType::LOCATION;
return;

case MAV_MOUNT_MODE_HOME_LOCATION:
Expand All @@ -1072,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) {
Expand All @@ -1087,6 +1097,9 @@ void AP_Mount_Backend::send_target_to_gimbal()
case MountTargetType::NEUTRAL:
send_target_neutral();
return;
case MountTargetType::LOCATION:
send_target_location(_roi_target);
return;
}
return; // should not reach this as all cases return
}
Expand Down Expand Up @@ -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");
Expand Down
5 changes: 4 additions & 1 deletion libraries/AP_Mount/AP_Mount_Backend.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -243,6 +243,7 @@ class AP_Mount_Backend
RATE = 1,
RETRACTED = 2,
NEUTRAL = 3,
LOCATION = 4,
};

// class for a single angle or rate target
Expand Down Expand Up @@ -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 {
Expand Down Expand Up @@ -398,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; // True if there is a pending clear ROI request
Comment thread
rmackay9 marked this conversation as resolved.

private:

Expand Down
27 changes: 27 additions & 0 deletions libraries/AP_Mount/AP_Mount_MAVLink.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -136,6 +136,9 @@ 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;

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",
info.vendor_name,
Expand Down Expand Up @@ -277,4 +280,28 @@ 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();
Copy link
Copy Markdown
Contributor

@peterbarker peterbarker Feb 10, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This does raise the question as to how to handle frames.

      <entry value="131072" name="GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL">
        <description>Gimbal manager supports to point to a global latitude, longitude, altitude position.</description>
      </entry>

That doesn't give any sort of frame - so when we go to add the same thing for GIMBAL_DEVICE we need to pay a bit of attention.

One would usually assume AMSL here, but it's possible you could say "terrain" and let the gimbal sort itself out with a laser rangefinder.

Any suggestions, @hamishwillee ?

I think for the time being throwing the problem over to the camera like this will be fine.

} 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
16 changes: 14 additions & 2 deletions libraries/AP_Mount/AP_Mount_MAVLink.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,11 +40,19 @@ 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 (

uint8_t supported_target = (
(1U<<unsigned(MountTargetType::ANGLE)) |
(1U<<unsigned(MountTargetType::RATE)) |
(1U<<unsigned(MountTargetType::RETRACTED))
);
);

// temporary hack until we get GIMBAL_DEVICE_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL
if ((strncmp(vendor_name, "AVTA", 4) == 0) && (strncmp(model_name, "CM41", 4) != 0)){
supported_target |= (1U<<unsigned(MountTargetType::LOCATION));
}

return supported_target;
};

// get attitude as a quaternion. returns true on success
Expand All @@ -71,6 +79,8 @@ 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;

void send_target_location(const Location &roi_loc) override;
Comment thread
KSimeonAVTA marked this conversation as resolved.

// internal variables
bool _got_device_info; // true once gimbal has provided device info
bool _initialised; // true once the gimbal has provided a GIMBAL_DEVICE_INFORMATION
Expand All @@ -80,5 +90,7 @@ 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[MAVLINK_MSG_GIMBAL_DEVICE_INFORMATION_FIELD_VENDOR_NAME_LEN]; // vendor name
char model_name[MAVLINK_MSG_GIMBAL_DEVICE_INFORMATION_FIELD_MODEL_NAME_LEN]; // model name
};
#endif // HAL_MOUNT_MAVLINK_ENABLED
1 change: 1 addition & 0 deletions libraries/AP_Mount/AP_Mount_Servo.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -110,6 +110,7 @@ void AP_Mount_Servo::update_angle_outputs(const MountAngleTarget& angle_rad)
return;
case MountTargetType::ANGLE:
case MountTargetType::RATE:
case MountTargetType::LOCATION:
break;
}

Expand Down
Loading