From c0f0e3eca3da4de3b27f87f22022b4cc989d51e9 Mon Sep 17 00:00:00 2001 From: Matt Date: Wed, 20 Dec 2017 16:59:49 -0500 Subject: [PATCH] COPTER: Fix MOUNT_CONTROL yaw in guided If the mount instance does not support yaw/pan, the copter needs to yaw in response to MSG_MOUNT_CONTROL and MAV_COMMAND_DO_MOUNT_CONTROL commands from a GCS or co-computer. There was no checking for mount pan in the GCS_Mavlink logic. As such, no yaw takes place when a mount control command calls for it. This patch impliments copter yaw control for both MSG_MOUNT_CONTROL and MAV_COMMAND_DO_MOUNT_CONTROL in copter GCS_Mavlink. --- ArduCopter/GCS_Mavlink.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index be4af96095..32218b1267 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1016,6 +1016,9 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) case MAV_CMD_DO_MOUNT_CONTROL: #if MOUNT == ENABLED + if(!copter.camera_mount.has_pan_control()) { + copter.set_auto_yaw_look_at_heading((float)packet.param3 / 100.0f,0.0f,0,0); + } copter.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7); result = MAV_RESULT_ACCEPTED; #endif @@ -1655,6 +1658,9 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) break; //deprecated. Use MAV_CMD_DO_MOUNT_CONTROL case MAVLINK_MSG_ID_MOUNT_CONTROL: + if(!copter.camera_mount.has_pan_control()) { + copter.set_auto_yaw_look_at_heading(mavlink_msg_mount_control_get_input_c(msg)/100.0f, 0.0f, 0, 0); + } copter.camera_mount.control_msg(msg); break; #endif // MOUNT == ENABLED