AP_Mount: consume gimbal device id for DO_SET_ROI_LOCATION/NONE #31743
Conversation
peterbarker
force-pushed
the
fix-roi-gimbal-device-id
branch
from
daad512 to
b5a5f90
Compare
peterbarker
left a comment
peterbarker
left a comment
There was a problem hiding this comment.
This should be at least two commits.
One which moves the code into AP_Mount and then one which adds the functionality.
And I do mean move.
And I'm very much not impressed with the population of the Location object in the mount library when you discovered that location_from_command_t wasn't available.
libraries/AP_Mount/AP_Mount.cpp
Outdated
| // 2nd gimbal, etc | ||
| const uint8_t gimbal_device_id = packet.param1; | ||
|
|
||
| Location target_loc; |
There was a problem hiding this comment.
Use location_from_command_t here.
You can't assume the pass-in location's altitude is relative-to-home.
There was a problem hiding this comment.
Reworked as two commits:
- separate handlers for DO_SET_ROI_LOCATION/NONE using location_from_command_t
- add gimbal device id support
tested in SITL with MNT1_TYPE=1.
deepak61296
force-pushed
the
fix-roi-gimbal-device-id
branch
from
b5a5f90 to
7df7126
Compare
peterbarker
left a comment
peterbarker
left a comment
There was a problem hiding this comment.
This is much better than the first one, thanks!
I think you're breaking ArduCopter's "yaw the vehicle if the mount can't yaw" behaviour here, and at the very least not supporting that in your new functions here.
That's the reason that MAV_CMD_DO_SET_ROI_LOCATION and NONE don't require mount to be enabled....
libraries/GCS_MAVLink/GCS_Common.cpp
Outdated
| // x : lat | ||
| // y : lon | ||
| // z : alt | ||
| const uint8_t gimbal_device_id = packet.param1; |
There was a problem hiding this comment.
This is a bit tricky as NaN can be passed in these packets. If a GCS does then SITL will blow up here with a numeric exception of some type or another.
There was a problem hiding this comment.
Thanks for the feedback!
fixed both issues:
-
vehicle yaw behavior preserved - the new handlers now fall back to
handle_command_do_set_roi(Location)when gimbal_device_id is 0/unspecified, so ROI commands work even without HAL_MOUNT_ENABLED (vehicle can still yaw toward ROI). -
NaN protection added** - Added
!isnan(packet.param1)check before using param1 as gimbal_device_id to prevent SITL crashes.
the gimbal-specific path (mount->set_roi_target()) is only taken when param1 >= 1 AND is not NaN.
Tested in SITL with and without MNT1_TYPE enabled - all commands return ACCEPTED.
Split DO_SET_ROI_LOCATION and DO_SET_ROI_NONE handling from DO_SET_ROI. These commands have different param1 semantics: - DO_SET_ROI: param1 is ROI mode - DO_SET_ROI_LOCATION/NONE: param1 is gimbal device id This commit adds dedicated handlers but still falls back to the existing handle_command_do_set_roi(Location) for vehicle handling. Gimbal device id support will be added separately.
Handle param1 (gimbal device id) for DO_SET_ROI_LOCATION and DO_SET_ROI_NONE commands when a specific gimbal is targeted (gimbal_device_id >= 1). When param1 is 0, NaN, or negative, fall back to vehicle ROI handling via the existing virtual handle_command_do_set_roi(Location) function. This preserves ArduCopter's ability to yaw the vehicle toward an ROI even when no gimbal is present. The NaN check prevents crashes in SITL when a GCS sends NaN as the gimbal device id parameter.
deepak61296
force-pushed
the
fix-roi-gimbal-device-id
branch
from
7df7126 to
311e56e
Compare
2 participants
Handle param1 (gimbal device id) for DO_SET_ROI_LOCATION and DO_SET_ROI_NONE immediate commands. Previously these commands always targeted the primary gimbal regardless of the device id.