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 1 commit
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
25 changes: 23 additions & 2 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 @@ -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);
}

Comment thread
KSimeonAVTA marked this conversation as resolved.
Outdated
// optionally set RC_TARGETING yaw lock state
if (option_set(Options::RCTARGETING_LOCK_FROM_PREVMODE)) {
set_yaw_lock(true);
Expand Down Expand Up @@ -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);
}
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 shouldn't be here - it should be handled by the output stage

Copy link
Copy Markdown
Contributor Author

@KSimeonAVTA KSimeonAVTA Feb 10, 2026

Choose a reason for hiding this comment

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

In clear_roi_target(), mode is set back to default mode, mount target type will no longer be LOCATION therefore send_target_location with zero target (ROI_NONE) will not be sent to gimbal.

Copy link
Copy Markdown
Contributor

@peterbarker peterbarker Feb 11, 2026

Choose a reason for hiding this comment

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

In clear_roi_target(), mode is set back to default mode, mount target type will no longer be LOCATION therefore send_target_location with zero target (ROI_NONE) will not be sent to gimbal.

Right - but we should be in some other targetting mode (e.g. ANGLE) and be sending angles (or retract, or...)

Copy link
Copy Markdown
Contributor Author

@KSimeonAVTA KSimeonAVTA Feb 12, 2026

Choose a reason for hiding this comment

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

Moved the clearing of roi in send_target_to_gimbal. but it is slightly different from what you have suggested. I am not checking the previous target type, only checking if there is a pending clear_roi request.
Because in the instance when ROI target is set and nudged pitch/yaw, then if clear_roi is requested, previous target type will not be location anymore.

}

// set_sys_target - sets system that mount should attempt to point towards
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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
}
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
2 changes: 2 additions & 0 deletions 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
29 changes: 29 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,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);
Comment thread
KSimeonAVTA marked this conversation as resolved.
Outdated

// 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 +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;
Comment thread
KSimeonAVTA marked this conversation as resolved.
Outdated
pkt.target_system = _sysid;
pkt.target_component = _compid;

if (roi_loc.initialised())
{
Comment thread
KSimeonAVTA marked this conversation as resolved.
Outdated
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;
}
Comment thread
KSimeonAVTA marked this conversation as resolved.
Outdated

_link->send_message(MAVLINK_MSG_ID_COMMAND_INT, (const char*)&pkt);
}

#endif // HAL_MOUNT_MAVLINK_ENABLED
17 changes: 12 additions & 5 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,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<<unsigned(MountTargetType::ANGLE)) |
(1U<<unsigned(MountTargetType::RATE)) |
(1U<<unsigned(MountTargetType::RETRACTED))
);

uint8_t supported_target = (1U<<unsigned(MountTargetType::ANGLE)) |
(1U<<unsigned(MountTargetType::RATE)) |
(1U<<unsigned(MountTargetType::RETRACTED));
Comment thread
KSimeonAVTA marked this conversation as resolved.
Outdated

if (strncmp(_vendor_name, "AVTA", 4) == 0)
Comment thread
KSimeonAVTA marked this conversation as resolved.
Outdated
supported_target |= (1U<<unsigned(MountTargetType::LOCATION));

return supported_target;
};

// get attitude as a quaternion. returns true on success
Expand All @@ -71,6 +75,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 +86,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
Comment thread
KSimeonAVTA marked this conversation as resolved.
Outdated
};
#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