From f300df5fd196504d7671440e6e231355ca56b796 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 12 Jul 2013 11:56:04 +1000 Subject: [PATCH] Plane: added FS_LONG_TIMEOUT and FS_SHORT_TIMEOUT parameters this is to address the issue raised here: http://diydrones.com/forum/topics/ardupilot-circle-mode --- ArduPlane/Parameters.h | 4 ++++ ArduPlane/Parameters.pde | 22 ++++++++++++++++++++-- ArduPlane/defines.h | 2 -- ArduPlane/radio.pde | 2 +- ArduPlane/system.pde | 21 ++++++++++----------- 5 files changed, 35 insertions(+), 16 deletions(-) diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 2cfd62f926..b22b5ca9f6 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -191,6 +191,8 @@ public: k_param_rc_12, k_param_fs_batt_voltage, k_param_fs_batt_mah, + k_param_short_fs_timeout, + k_param_long_fs_timeout, // // 200: Feed-forward gains @@ -313,6 +315,8 @@ public: // Failsafe AP_Int8 short_fs_action; AP_Int8 long_fs_action; + AP_Float short_fs_timeout; + AP_Float long_fs_timeout; AP_Int8 gcs_heartbeat_fs_enabled; AP_Float fs_batt_voltage; AP_Float fs_batt_mah; diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 591ed85341..9ee1311803 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -357,18 +357,36 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: FS_SHORT_ACTN // @DisplayName: Short failsafe action - // @Description: The action to take on a short (1.5 seconds) failsafe event in AUTO, GUIDED or LOITER modes. A short failsafe event in stabilization modes will always cause a change to CIRCLE mode. In AUTO mode you can choose whether it will RTL (ReturnToLaunch) or continue with the mission. If FS_SHORT_ACTN is 0 then it will continue with the mission, if it is 1 then it will enter CIRCLE mode, and then enter RTL if the failsafe condition persists for 20 seconds. + // @Description: The action to take on a short (FS_SHORT_TIMEOUT) failsafe event in AUTO, GUIDED or LOITER modes. A short failsafe event in stabilization modes will always cause an immediate change to CIRCLE mode. In AUTO mode you can choose whether it will RTL (ReturnToLaunch) or continue with the mission. If FS_SHORT_ACTN is 0 then it will continue with the mission, if it is 1 then it will enter CIRCLE mode, and then enter RTL if the failsafe condition persists for FS_LONG_TIMEOUT seconds. // @Values: 0:Continue,1:Circle/ReturnToLaunch // @User: Standard GSCALAR(short_fs_action, "FS_SHORT_ACTN", SHORT_FAILSAFE_ACTION), + // @Param: FS_SHORT_TIMEOUT + // @DisplayName: Short failsafe timeout + // @Description: The time in seconds that a failsafe condition has to persist before a short failsafe event will occor. This defaults to 1.5 seconds + // @Units: seconds + // @Range: 1 100 + // @Increment: 0.5 + // @User: Standard + GSCALAR(short_fs_timeout, "FS_SHORT_TIMEOUT", 1.5f), + // @Param: FS_LONG_ACTN // @DisplayName: Long failsafe action - // @Description: The action to take on a long (20 second) failsafe event in AUTO, GUIDED or LOITER modes. A long failsafe event in stabilization modes will always cause an RTL (ReturnToLaunch). In AUTO modes you can choose whether it will RTL or continue with the mission. If FS_LONG_ACTN is 0 then it will continue with the mission, if it is 1 then it will enter RTL mode. Note that if FS_SHORT_ACTN is 1, then the aircraft will enter CIRCLE mode after 1.5 seconds of failsafe, and will always enter RTL after 20 seconds of failsafe, regardless of the FS_LONG_ACTN setting. + // @Description: The action to take on a long (FS_LONG_TIMEOUT seconds) failsafe event in AUTO, GUIDED or LOITER modes. A long failsafe event in stabilization modes will always cause an RTL (ReturnToLaunch). In AUTO modes you can choose whether it will RTL or continue with the mission. If FS_LONG_ACTN is 0 then it will continue with the mission, if it is 1 then it will enter RTL mode. Note that if FS_SHORT_ACTN is 1, then the aircraft will enter CIRCLE mode after FS_SHORT_TIMEOUT seconds of failsafe, and will always enter RTL after FS_LONG_TIMEOUT seconds of failsafe, regardless of the FS_LONG_ACTN setting. // @Values: 0:Continue,1:ReturnToLaunch // @User: Standard GSCALAR(long_fs_action, "FS_LONG_ACTN", LONG_FAILSAFE_ACTION), + // @Param: FS_LONG_TIMEOUT + // @DisplayName: Long failsafe timeout + // @Description: The time in seconds that a failsafe condition has to persist before a long failsafe event will occor. This defaults to 20 seconds + // @Units: seconds + // @Range: 1 300 + // @Increment: 0.5 + // @User: Standard + GSCALAR(long_fs_timeout, "FS_LONG_TIMEOUT", 20), + // @Param: FS_BATT_VOLTAGE // @DisplayName: Failsafe battery voltage // @Description: Battery voltage to trigger failsafe. Set to 0 to disable battery voltage failsafe. If the battery voltage drops below this voltage then the plane will RTL diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index e175aa1e98..805a80eaed 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -22,8 +22,6 @@ #define FAILSAFE_SHORT 1 #define FAILSAFE_LONG 2 #define FAILSAFE_GCS 3 -#define FAILSAFE_SHORT_TIME 1500 // Miliiseconds -#define FAILSAFE_LONG_TIME 20000 // Miliiseconds // active altitude sensor diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index a4a971c676..8549ea60bd 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -140,7 +140,7 @@ static void control_failsafe(uint16_t pwm) // Check for failsafe condition based on loss of GCS control if (rc_override_active) { - if (millis() - last_heartbeat_ms > FAILSAFE_SHORT_TIME) { + if (millis() - last_heartbeat_ms > g.short_fs_timeout*1000) { ch3_failsafe = true; } else { ch3_failsafe = false; diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index d103511b78..ea4da0f749 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -402,28 +402,27 @@ static void check_long_failsafe() // only act on changes // ------------------- if(failsafe != FAILSAFE_LONG && failsafe != FAILSAFE_GCS) { - if (rc_override_active && tnow - last_heartbeat_ms > FAILSAFE_LONG_TIME) { + if (rc_override_active && (tnow - last_heartbeat_ms) > g.long_fs_timeout*1000) { failsafe_long_on_event(FAILSAFE_LONG); - } - if(!rc_override_active && failsafe == FAILSAFE_SHORT && - (tnow - ch3_failsafe_timer) > FAILSAFE_LONG_TIME) { + } else if (!rc_override_active && failsafe == FAILSAFE_SHORT && + (tnow - ch3_failsafe_timer) > g.long_fs_timeout*1000) { failsafe_long_on_event(FAILSAFE_LONG); - } - if (g.gcs_heartbeat_fs_enabled && + } else if (g.gcs_heartbeat_fs_enabled && last_heartbeat_ms != 0 && - (tnow - last_heartbeat_ms) > FAILSAFE_LONG_TIME) { + (tnow - last_heartbeat_ms) > g.long_fs_timeout*1000) { failsafe_long_on_event(FAILSAFE_GCS); } } else { // We do not change state but allow for user to change mode if (failsafe == FAILSAFE_GCS && - (tnow - last_heartbeat_ms) < FAILSAFE_SHORT_TIME) + (tnow - last_heartbeat_ms) < g.short_fs_timeout*1000) { failsafe = FAILSAFE_NONE; - if (failsafe == FAILSAFE_LONG && rc_override_active && - (tnow - last_heartbeat_ms) < FAILSAFE_SHORT_TIME) + } else if (failsafe == FAILSAFE_LONG && rc_override_active && + (tnow - last_heartbeat_ms) < g.short_fs_timeout*1000) { failsafe = FAILSAFE_NONE; - if (failsafe == FAILSAFE_LONG && !rc_override_active && !ch3_failsafe) + } else if (failsafe == FAILSAFE_LONG && !rc_override_active && !ch3_failsafe) { failsafe = FAILSAFE_NONE; + } } }