From 3642b14a0b7167666f9ab2dd3d6ed8b71c21d0ec Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Thu, 9 Sep 2021 00:05:49 -0400 Subject: [PATCH] ArduCopter: Guided pos_control_run add yaw_rate timeout --- ArduCopter/Parameters.cpp | 2 +- ArduCopter/mode.h | 2 +- ArduCopter/mode_guided.cpp | 10 +++++++++- 3 files changed, 11 insertions(+), 3 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 5cfe9b4a0c..6554b03220 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1060,7 +1060,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { #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 + // @Description: Guided mode timeout after which vehicle will stop or return to level 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.1 5 // @User: Advanced diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 47bf7241c1..00692f8515 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -923,7 +923,7 @@ public: void angle_control_start(); void angle_control_run(); - // return guided mode timeout in milliseconds. Only used for velocity, acceleration and angle control + // return guided mode timeout in milliseconds. Only used for velocity, acceleration, angle control, and angular rate control uint32_t get_timeout_ms() const; bool use_pilot_yaw() const override; diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 5ddde597e8..f08bb3b579 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -468,6 +468,7 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y guided_pos_terrain_alt = terrain_alt; guided_vel_target_cms.zero(); guided_accel_target_cmss.zero(); + update_time_ms = millis(); // log target copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); @@ -678,6 +679,13 @@ void ModeGuided::pos_control_run() guided_accel_target_cmss.zero(); guided_vel_target_cms.zero(); + // stop rotating if no updates received within timeout_ms + if (millis() - update_time_ms > get_timeout_ms()) { + if ((auto_yaw.mode() == AUTO_YAW_RATE) || (auto_yaw.mode() == AUTO_YAW_ANGLE_RATE)) { + auto_yaw.set_rate(0.0f); + } + } + float pos_offset_z_buffer = 0.0; // Vertical buffer size in m if (guided_pos_terrain_alt) { pos_offset_z_buffer = MIN(copter.wp_nav->get_terrain_margin() * 100.0, 0.5 * fabsf(guided_pos_target_cm.z)); @@ -1163,7 +1171,7 @@ float ModeGuided::crosstrack_error() const return 0; } -// return guided mode timeout in milliseconds. Only used for velocity, acceleration and angle control +// return guided mode timeout in milliseconds. Only used for velocity, acceleration, angle control, and angular rates uint32_t ModeGuided::get_timeout_ms() const { return MAX(copter.g2.guided_timeout, 0.1) * 1000;