AP_Mount: gremsy integrates find_by_mavtype_and_compid

This commit is contained in:
Randy Mackay 2022-09-07 11:19:20 +09:00
parent c034f8a099
commit 7c9b487779
1 changed files with 6 additions and 10 deletions

View File

@ -150,16 +150,12 @@ void AP_Mount_Gremsy::find_gimbal()
// search for a mavlink enabled gimbal // search for a mavlink enabled gimbal
if (!_found_gimbal) { if (!_found_gimbal) {
mavlink_channel_t chan; // we expect that instance 0 has compid = MAV_COMP_ID_GIMBAL, instance 1 has compid = MAV_COMP_ID_GIMBAL2, etc
uint8_t sysid, compid; uint8_t compid = (_instance == 0) ? MAV_COMP_ID_GIMBAL : MAV_COMP_ID_GIMBAL2 + (_instance - 1);
if (GCS_MAVLINK::find_by_mavtype(MAV_TYPE_GIMBAL, sysid, compid, chan)) { if (GCS_MAVLINK::find_by_mavtype_and_compid(MAV_TYPE_GIMBAL, compid, _sysid, _chan)) {
if (((_instance == 0) && (compid == MAV_COMP_ID_GIMBAL)) ||
((_instance == 1) && (compid == MAV_COMP_ID_GIMBAL2))) {
_found_gimbal = true;
_sysid = sysid;
_compid = compid; _compid = compid;
_chan = chan; _found_gimbal = true;
} return;
} else { } else {
// have not yet found a gimbal so return // have not yet found a gimbal so return
return; return;