AP_Mount_MAVLink: forward MAV_CMD_DO_SET_ROI_LOCATION to gimbal#32162
Conversation
| if (natively_supports(MountTargetType::LOCATION)) { | ||
| send_target_location(_roi_target); | ||
| } |
There was a problem hiding this comment.
This shouldn't be here - it should be handled by the output stage
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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...)
There was a problem hiding this comment.
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.
| 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(); |
There was a problem hiding this comment.
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.
|
I've created mavlink/mavlink#2411 to track adding a capability flag (which, presumably, the AVT cameras may start to emit) |
|
Note this comment in the top of ... this patch is sending that message to the gimbal device. Which AFAICS is the most sensible, obvious thing for ArduPilot to be doing. @hamishwillee were these restrictions aimed at GCSs rather than the autopilot (gimbal manager)? |
peterbarker
left a comment
There was a problem hiding this comment.
LGTM.
Please state what testing this has had.
|
@peterbarker Re I believe that the comments are correct, and that they are aimed at both the GCS AND the Gimbal manager. The command should not be send to the gimbal device. The gimbal interface is (nearly) cleanly divided into manager and device commands/messages. The high level commands like this one should be sent only to the gimbal manager or vehicle. They then get translated to lower level commands that a gimbal device might implement. In other words, a gimbal device should not know about this command. It is possible I am wrong. @julianoes Can confirm. |
|
Tested the following using AVT gimbal, QGC daily build (Feb 10, 2026) |
That was my idea but given the additional flags where a device can follow a global lat/lon, this model falls apart and we now have to send ROI commands to the device presumably. I have the feeling this is getting a bit messy, and maybe that gimbal device should have been a gimbal manager in the first place. If it is a gimbal manager, then the autopilot is no longer in control of the input command deconfliction, but that gimbal is, and maybe that's where the current design falls apart. Not sure what to do. The design was meant to please all the various use cases and setups but clearly doesn't actually do so. |
…target is supported
510447e to
c756770
Compare
|
Merged, thanks! |
Forwards MAV_CMD_DO_SET_ROI_LOCATION to AVT gimbal.
Temporarily use vendor name to identify whether target location is supported.