diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index f5802d65725f01..6170ce0096e723 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -358,6 +358,12 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com return MAV_RESULT_ACCEPTED; } + // if neither angles nor rates were provided set the RC_TARGETING yaw lock state + if (isnan(pitch_angle_deg) && isnan(yaw_angle_deg) && isnan(pitch_rate_degs) && isnan(yaw_rate_degs)) { + backend->set_yaw_lock(flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK); + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; } @@ -498,6 +504,12 @@ void AP_Mount::handle_gimbal_manager_set_pitchyaw(const mavlink_message_t &msg) backend->set_rate_target(0, pitch_rate_degs, yaw_rate_degs, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK); return; } + + // if neither angles nor rates were provided set the RC_TARGETING yaw lock state + if (isnan(packet.pitch) && isnan(packet.yaw) && isnan(packet.pitch_rate) && isnan(packet.yaw_rate)) { + backend->set_yaw_lock(flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK); + return; + } } MAV_RESULT AP_Mount::handle_command_do_set_roi_sysid(const mavlink_command_int_t &packet)