From be29b231954d79076aa59e3b3cc006fbfcc2763a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 10 Mar 2021 12:28:29 +0900 Subject: [PATCH] Copter: do-mount-control ignored if no mount Co-authored-by: srirajshukla Co-authored-by: Yash Vadi --- ArduCopter/GCS_Mavlink.cpp | 15 +++++++++------ ArduCopter/mode_auto.cpp | 7 +++++-- 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index b59ca4ecdd..2668608615 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -685,15 +685,17 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_long_t &packet) { - // if the mount doesn't do pan control then yaw the entire vehicle instead: switch (packet.command) { #if HAL_MOUNT_ENABLED case MAV_CMD_DO_MOUNT_CONTROL: - if (!copter.camera_mount.has_pan_control()) { + // if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead + if ((copter.camera_mount.get_mount_type() != copter.camera_mount.MountType::Mount_Type_None) && + !copter.camera_mount.has_pan_control()) { copter.flightmode->auto_yaw.set_fixed_yaw( (float)packet.param3 * 0.01f, 0.0f, - 0,0); + 0, + false); } break; #endif @@ -955,13 +957,14 @@ void GCS_MAVLINK_Copter::handle_mount_message(const mavlink_message_t &msg) switch (msg.msgid) { #if HAL_MOUNT_ENABLED case MAVLINK_MSG_ID_MOUNT_CONTROL: - if (!copter.camera_mount.has_pan_control()) { - // if the mount doesn't do pan control then yaw the entire vehicle instead: + // if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead + if ((copter.camera_mount.get_mount_type() != copter.camera_mount.MountType::Mount_Type_None) && + !copter.camera_mount.has_pan_control()) { copter.flightmode->auto_yaw.set_fixed_yaw( mavlink_msg_mount_control_get_input_c(&msg) * 0.01f, 0.0f, 0, - 0); + false); break; } diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 0e76652d2a..0352c2a41b 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -1452,9 +1452,12 @@ void ModeAuto::do_roi(const AP_Mission::Mission_Command& cmd) void ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd) { #if HAL_MOUNT_ENABLED - if (!copter.camera_mount.has_pan_control()) { - auto_yaw.set_fixed_yaw(cmd.content.mount_control.yaw,0.0f,0,0); + // if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead + if ((copter.camera_mount.get_mount_type() != copter.camera_mount.MountType::Mount_Type_None) && + !copter.camera_mount.has_pan_control()) { + auto_yaw.set_fixed_yaw(cmd.content.mount_control.yaw,0.0f,0,false); } + // pass the target angles to the camera mount copter.camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw); #endif }