From 50d05929620fc27debcd997e3f70888d2b98ebf9 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 9 Jul 2021 14:02:07 +0900
Subject: [PATCH] Copter: add GUID_TIMEOUT for guided mode vel, accel and angle
 control

---
 ArduCopter/Parameters.cpp  | 10 ++++++++++
 ArduCopter/Parameters.h    |  4 ++++
 ArduCopter/mode.h          |  3 +++
 ArduCopter/mode_guided.cpp | 17 ++++++++++-------
 4 files changed, 27 insertions(+), 7 deletions(-)

diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp
index 418370b34b..c5e580e301 100644
--- a/ArduCopter/Parameters.cpp
+++ b/ArduCopter/Parameters.cpp
@@ -1069,6 +1069,16 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     AP_GROUPINFO("RNGFND_FILT", 45, ParametersG2, rangefinder_filt, RANGEFINDER_FILT_DEFAULT),
 #endif
 
+#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
+    // @Units: s
+    // @Range: 0.1 5
+    // @User: Advanced
+    AP_GROUPINFO("GUID_TIMEOUT", 46, ParametersG2, guided_timeout, 3.0),
+#endif
+
     AP_GROUPEND
 };
 
diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h
index e103667872..2432455a26 100644
--- a/ArduCopter/Parameters.h
+++ b/ArduCopter/Parameters.h
@@ -649,6 +649,10 @@ public:
     AP_Float rangefinder_filt;
 #endif
 
+#if MODE_GUIDED_ENABLED == ENABLED
+    AP_Float guided_timeout;
+#endif
+
 };
 
 extern const AP_Param::Info        var_info[];
diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
index 04bb224b47..16a94eec43 100644
--- a/ArduCopter/mode.h
+++ b/ArduCopter/mode.h
@@ -890,6 +890,9 @@ public:
     void angle_control_start();
     void angle_control_run();
 
+    // return guided mode timeout in milliseconds.  Only used for velocity, acceleration and angle control
+    uint32_t get_timeout_ms() const;
+
 protected:
 
     const char *name() const override { return "GUIDED"; }
diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp
index 3f33175f58..7f70ad9f12 100644
--- a/ArduCopter/mode_guided.cpp
+++ b/ArduCopter/mode_guided.cpp
@@ -10,9 +10,6 @@
  # define GUIDED_LOOK_AT_TARGET_MIN_DISTANCE_CM     500     // point nose at target if it is more than 5m away
 #endif
 
-#define GUIDED_POSVEL_TIMEOUT_MS    3000    // guided mode's position-velocity controller times out after 3seconds with no new updates
-#define GUIDED_ATTITUDE_TIMEOUT_MS  1000    // guided mode's attitude controller times out after 1 second with no new updates
-
 static Vector3p guided_pos_target_cm;       // position target (used by posvel controller only)
 bool guided_pos_terrain_alt;                // true if guided_pos_target_cm.z is an alt above terrain
 static Vector3f guided_vel_target_cms;      // velocity target (used by pos_vel_accel controller and vel_accel controller)
@@ -604,7 +601,7 @@ void ModeGuided::accel_control_run()
 
     // set velocity to zero and stop rotating if no updates received for 3 seconds
     uint32_t tnow = millis();
-    if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
+    if (tnow - update_time_ms > get_timeout_ms()) {
         guided_vel_target_cms.zero();
         guided_accel_target_cmss.zero();
         if (auto_yaw.mode() == AUTO_YAW_RATE) {
@@ -667,7 +664,7 @@ void ModeGuided::velaccel_control_run()
 
     // set velocity to zero and stop rotating if no updates received for 3 seconds
     uint32_t tnow = millis();
-    if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
+    if (tnow - update_time_ms > get_timeout_ms()) {
         guided_vel_target_cms.zero();
         guided_accel_target_cmss.zero();
         if (auto_yaw.mode() == AUTO_YAW_RATE) {
@@ -742,7 +739,7 @@ void ModeGuided::posvelaccel_control_run()
 
     // set velocity to zero and stop rotating if no updates received for 3 seconds
     uint32_t tnow = millis();
-    if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
+    if (tnow - update_time_ms > get_timeout_ms()) {
         guided_vel_target_cms.zero();
         guided_accel_target_cmss.zero();
         if (auto_yaw.mode() == AUTO_YAW_RATE) {
@@ -827,7 +824,7 @@ void ModeGuided::angle_control_run()
 
     // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
     uint32_t tnow = millis();
-    if (tnow - guided_angle_state.update_time_ms > GUIDED_ATTITUDE_TIMEOUT_MS) {
+    if (tnow - guided_angle_state.update_time_ms > get_timeout_ms()) {
         roll_in = 0.0f;
         pitch_in = 0.0f;
         climb_rate_cms = 0.0f;
@@ -1037,4 +1034,10 @@ float ModeGuided::crosstrack_error() const
     return 0;
 }
 
+// return guided mode timeout in milliseconds.  Only used for velocity, acceleration and angle control
+uint32_t ModeGuided::get_timeout_ms() const
+{
+    return MAX(copter.g2.guided_timeout, 0.1) * 1000;
+}
+
 #endif