ArduPlane: make Guided update frequency limit configurable

ArduPilot · Nov 5, 2024 · 6fd68bc · 6fd68bc
1 parent 2892483
commit 6fd68bc
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 4 deletions.
diff --git 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
@@ -566,6 +566,8 @@ class ParametersG2 {
     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
@@ -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);