From 53879013090f7dcdb75d267c6adeffabd67f7461 Mon Sep 17 00:00:00 2001 From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Date: Tue, 24 Dec 2024 11:01:10 -0700 Subject: [PATCH] ArduPlane: add guided timeout * Preserve default of 3s Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> --- ArduPlane/Parameters.cpp | 8 ++++++++ ArduPlane/Parameters.h | 2 ++ ArduPlane/mode.h | 4 ++++ ArduPlane/mode_guided.cpp | 11 ++++++++--- 4 files changed, 22 insertions(+), 3 deletions(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 78d30542f76af..1abf4177f45cf 100644 --- 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 index e7cc5326f4280..91750a3135e42 100644 --- 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 index 213e547c6a668..241716e8296e1 100644 --- 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 index a97bedcd7dffb..79f53b6a04f95 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 < 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; +}