From 6fd68bc3119330f47f2a4e7b782a5410638aaca4 Mon Sep 17 00:00:00 2001
From: Tim Tuxworth <tim@objectivedata.com>
Date: Tue, 5 Nov 2024 11:15:17 +0800
Subject: [PATCH] ArduPlane: make Guided update frequency limit configurable

---
 ArduPlane/Parameters.cpp  | 9 ++++++++-
 ArduPlane/Parameters.h    | 2 ++
 ArduPlane/mode_guided.cpp | 6 +++---
 3 files changed, 13 insertions(+), 4 deletions(-)

diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp
index d643d8b013..0ad83ce6d5 100644
--- a/ArduPlane/Parameters.cpp
+++ b/ArduPlane/Parameters.cpp
@@ -1288,7 +1288,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("RNGFND_LND_ORNT", 36, ParametersG2, rangefinder_land_orient, ROTATION_PITCH_270),
 #endif
-    
+
+    // @Param: GUIDED_UPD_LIM
+    // @DisplayName: Guided Update Limit
+    // @Description: The maximum frequency that an guided mode commands sent by external system such as lua or mavlink can update roll, pitch and throttle.
+    // @Units: ms
+    // @User: Standard
+    AP_GROUPINFO("GUIDED_UPD_LIM", 37, ParametersG2, guided_update_frequency_limit, 3000),
+
     AP_GROUPEND
 };
 
diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
index 377eb9ded8..f185ce86d9 100644
--- a/ArduPlane/Parameters.h
+++ b/ArduPlane/Parameters.h
@@ -566,6 +566,8 @@ public:
     AP_Follow follow;
 #endif
 
+    AP_Int32        guided_update_frequency_limit;
+
     AP_Float        fs_ekf_thresh;
 
     // min initial climb in RTL
diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp
index a97bedcd7d..f0afd409ed 100644
--- a/ArduPlane/mode_guided.cpp
+++ b/ArduPlane/mode_guided.cpp
@@ -38,7 +38,7 @@ void ModeGuided::update()
 
     // Received an external msg that guides roll in the last 3 seconds?
     if (plane.guided_state.last_forced_rpy_ms.x > 0 &&
-            millis() - plane.guided_state.last_forced_rpy_ms.x < 3000) {
+            (millis() - plane.guided_state.last_forced_rpy_ms.x) < (uint32_t)plane.g2.guided_update_frequency_limit) {
         plane.nav_roll_cd = constrain_int32(plane.guided_state.forced_rpy_cd.x, -plane.roll_limit_cd, plane.roll_limit_cd);
         plane.update_load_factor();
 
@@ -76,7 +76,7 @@ void ModeGuided::update()
     }
 
     if (plane.guided_state.last_forced_rpy_ms.y > 0 &&
-            millis() - plane.guided_state.last_forced_rpy_ms.y < 3000) {
+            (millis() - plane.guided_state.last_forced_rpy_ms.y) < (uint32_t)plane.g2.guided_update_frequency_limit) {
         plane.nav_pitch_cd = constrain_int32(plane.guided_state.forced_rpy_cd.y, plane.pitch_limit_min*100, plane.aparm.pitch_limit_max.get()*100);
     } else {
         plane.calc_nav_pitch();
@@ -89,7 +89,7 @@ void ModeGuided::update()
 
     }  else if (plane.aparm.throttle_cruise > 1 &&
             plane.guided_state.last_forced_throttle_ms > 0 &&
-            millis() - plane.guided_state.last_forced_throttle_ms < 3000) {
+            (millis() - plane.guided_state.last_forced_throttle_ms) < (uint32_t)plane.g2.guided_update_frequency_limit) {
         // Received an external msg that guides throttle in the last 3 seconds?
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.guided_state.forced_throttle);