ArduPlane: add guided timeout

* Preserve default of 3s Signed-off-by: Ryan Friedman <[email protected]>
ArduPilot · Dec 24, 2024 · 5387901 · 5387901
1 parent 2851aaf
commit 5387901
Show file tree

Hide file tree

Showing 4 changed files with 22 additions and 3 deletions.
diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp
@@ -1303,6 +1303,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: GUID_TIMEOUT
+    // @DisplayName: Guided mode timeout
+    // @Description: Guided mode timeout after which vehicle will return to guided loiter 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.01 5
+    // @User: Advanced
+    AP_GROUPINFO("GUID_TIMEOUT", 37, ParametersG2, guided_timeout, 3.0),
 
     AP_GROUPEND
 };

diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
@@ -566,6 +566,8 @@ class ParametersG2 {
     // guided yaw heading PID
     AC_PID guidedHeading{5000.0,  0.0,   0.0, 0 ,  10.0,   5.0,  5.0 ,  5.0  , 0.0};
 #endif
+    AP_Float guided_timeout;
+
 
 #if AP_SCRIPTING_ENABLED && AP_FOLLOW_ENABLED
     AP_Follow follow;

diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
@@ -332,6 +332,10 @@ class ModeGuided : public Mode
 
     void update_target_altitude() override;
 
+    // Return guided mode timeout in milliseconds.
+    // Only used for velocity, acceleration, angle control, and angular rate control.
+    uint32_t get_timeout_ms() const;
+
 protected:
 
     bool _enter() override;

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 < get_timeout_ms()) {
         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 < get_timeout_ms()) {
         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 < get_timeout_ms()) {
         // 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);
 
@@ -158,3 +158,8 @@ void ModeGuided::update_target_altitude()
         Mode::update_target_altitude();
     }
 }
+
+uint32_t ModeGuided::get_timeout_ms() const
+{
+    return constrain_float(plane.g2.guided_timeout, 0.01, 5.0) * 1000;
+}