diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index 87c47d8a6f..aea8a5ceeb 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -481,10 +481,15 @@ void Copter::guided_vel_control_run() // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - // set velocity to zero if no updates received for 3 seconds + // set velocity to zero and stop rotating if no updates received for 3 seconds uint32_t tnow = millis(); - if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control->get_desired_velocity().is_zero()) { - guided_set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f,0.0f,0.0f)); + if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) { + if (!pos_control->get_desired_velocity().is_zero()) { + guided_set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f,0.0f,0.0f)); + } + if (auto_yaw_mode == AUTO_YAW_RATE) { + set_auto_yaw_rate(0.0f); + } } else { guided_set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms); } @@ -540,10 +545,13 @@ void Copter::guided_posvel_control_run() // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - // set velocity to zero if no updates received for 3 seconds + // set velocity to zero and stop rotating if no updates received for 3 seconds uint32_t tnow = millis(); - if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !guided_vel_target_cms.is_zero()) { + if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) { guided_vel_target_cms.zero(); + if (auto_yaw_mode == AUTO_YAW_RATE) { + set_auto_yaw_rate(0.0f); + } } // calculate dt