From 5dfd1ff8d6830f397b32a82e78582c1e57a5fe39 Mon Sep 17 00:00:00 2001
From: TShapinsky <tobiasshapinsky@gmail.com>
Date: Tue, 11 Aug 2015 16:36:42 -0700
Subject: [PATCH] Copter: add velocity control timeout in guided mode

---
 ArduCopter/control_guided.cpp | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp
index 22ae354830..6c24bf8f2c 100644
--- a/ArduCopter/control_guided.cpp
+++ b/ArduCopter/control_guided.cpp
@@ -15,6 +15,7 @@
 static Vector3f posvel_pos_target_cm;
 static Vector3f posvel_vel_target_cms;
 static uint32_t posvel_update_time_ms;
+static uint32_t vel_update_time_ms;
 
 struct Guided_Limit {
     uint32_t timeout_ms;  // timeout (in seconds) from the time that guided is invoked
@@ -138,6 +139,8 @@ void Copter::guided_set_velocity(const Vector3f& velocity)
         guided_vel_control_start();
     }
 
+    vel_update_time_ms = millis();
+
     // set position controller velocity target
     pos_control.set_desired_velocity(velocity);
 }
@@ -276,6 +279,12 @@ void Copter::guided_vel_control_run()
         }
     }
 
+    // set velocity to zero 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()) {
+        pos_control.set_desired_velocity(Vector3f(0,0,0));
+    }
+
     // calculate dt
     float dt = pos_control.time_since_last_xy_update();