ArduCopter: Guided pos_control_run add yaw_rate timeout

ArduCopter/Parameters.cpp

@@ -1060,7 +1060,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
 #if MODE_GUIDED_ENABLED == ENABLED
     // @Param: GUID_TIMEOUT
     // @DisplayName: Guided mode timeout
-    // @Description: Guided mode timeout after which vehicle will stop or return to level if no updates are received from caller.  Only applicable during velocity, acceleration or angle control
+    // @Description: Guided mode timeout after which vehicle will stop or return to level if no updates are received from caller. Only applicable during any combination of velocity, acceleration, angle control, and/or angular rate control
     // @Units: s
     // @Range: 0.1 5
     // @User: Advanced

ArduCopter/mode.h

@@ -923,7 +923,7 @@ public:
     void angle_control_start();
     void angle_control_run();
 
-    // return guided mode timeout in milliseconds.  Only used for velocity, acceleration and angle control
+    // return guided mode timeout in milliseconds. Only used for velocity, acceleration, angle control, and angular rate control
     uint32_t get_timeout_ms() const;
 
     bool use_pilot_yaw() const override;

ArduCopter/mode_guided.cpp

@@ -468,6 +468,7 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
     guided_pos_terrain_alt = terrain_alt;
     guided_vel_target_cms.zero();
     guided_accel_target_cmss.zero();
+    update_time_ms = millis();
 
     // log target
     copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss);
@@ -678,6 +679,13 @@ void ModeGuided::pos_control_run()
     guided_accel_target_cmss.zero();
     guided_vel_target_cms.zero();
 
+    // stop rotating if no updates received within timeout_ms
+    if (millis() - update_time_ms > get_timeout_ms()) {
+        if ((auto_yaw.mode() == AUTO_YAW_RATE) || (auto_yaw.mode() == AUTO_YAW_ANGLE_RATE)) {
+            auto_yaw.set_rate(0.0f);
+        }
+    }
+
     float pos_offset_z_buffer = 0.0; // Vertical buffer size in m
     if (guided_pos_terrain_alt) {
         pos_offset_z_buffer = MIN(copter.wp_nav->get_terrain_margin() * 100.0, 0.5 * fabsf(guided_pos_target_cm.z));
@@ -1163,7 +1171,7 @@ float ModeGuided::crosstrack_error() const
     return 0;
 }
 
-// return guided mode timeout in milliseconds.  Only used for velocity, acceleration and angle control
+// return guided mode timeout in milliseconds. Only used for velocity, acceleration, angle control, and angular rates
 uint32_t ModeGuided::get_timeout_ms() const
 {
     return MAX(copter.g2.guided_timeout, 0.1) * 1000;