From dfa3f2398e5e246e911e3644489014e0ec45dc0c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 11 Aug 2023 14:49:18 +0900 Subject: [PATCH] ExtLibs: setMountConfigure uses mav-cmd-do-mount-configure --- ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs | 21 ++++++++----------- 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs index 0d3926187c..62f814622a 100644 --- a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs +++ b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs @@ -4469,18 +4469,15 @@ public void setDigicamControl(bool shot) [Obsolete] public void setMountConfigure(MAV_MOUNT_MODE mountmode, bool stabroll, bool stabpitch, bool stabyaw) { - mavlink_mount_configure_t req = new mavlink_mount_configure_t(); - - req.target_system = MAV.sysid; - req.target_component = MAV.compid; - req.mount_mode = (byte) mountmode; - req.stab_pitch = (stabpitch == true) ? (byte) 1 : (byte) 0; - req.stab_roll = (stabroll == true) ? (byte) 1 : (byte) 0; - req.stab_yaw = (stabyaw == true) ? (byte) 1 : (byte) 0; - - generatePacket((byte) MAVLINK_MSG_ID.MOUNT_CONFIGURE, req); - Thread.Sleep(20); - generatePacket((byte) MAVLINK_MSG_ID.MOUNT_CONFIGURE, req); + byte stab_roll = (stabroll == true) ? (byte) 1 : (byte) 0; + byte stab_pitch = (stabpitch == true) ? (byte) 1 : (byte) 0; + byte stab_yaw = (stabyaw == true) ? (byte) 1 : (byte) 0; + + // p1 : mount mode + // p2, p3, p4 : stabilize roll, pitch, yaw + // p5, p6, p7 : empty + // no ack required + doCommand(MAV.sysid, MAV.compid, MAV_CMD.DO_MOUNT_CONFIGURE, (byte)mountmode, stab_roll, stab_pitch, stab_yaw, 0, 0, 0, false); } [Obsolete]