diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index d1fbeea9f0..45c99975db 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -1213,6 +1213,11 @@ static void update_flight_mode(void) if (inverted_flight) { nav_pitch_cd = -nav_pitch_cd; } + if (failsafe.ch3_failsafe && g.short_fs_action == 2) { + // FBWA failsafe glide + nav_roll_cd = 0; + nav_pitch_cd = 0; + } break; } diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 699c599574..f182ff1a2a 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -51,6 +51,12 @@ static bool stick_mixing_enabled(void) return false; } } + + if (failsafe.ch3_failsafe && g.short_fs_action == 2) { + // don't do stick mixing in FBWA glide mode + return false; + } + // non-auto mode. Always do stick mixing return true; } diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 49a91cb0e5..2da0973a1a 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -368,8 +368,8 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: FS_SHORT_ACTN // @DisplayName: Short failsafe action - // @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 enter CIRCLE mode 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 + // @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 enter CIRCLE mode 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. If it is set to 2 then the plane will enter FBWA mode with zero throttle and level attitude to glide in. + // @Values: 0:Continue,1:Circle/ReturnToLaunch,2:Glide // @User: Standard GSCALAR(short_fs_action, "FS_SHORT_ACTN", SHORT_FAILSAFE_ACTION), @@ -384,8 +384,8 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: FS_LONG_ACTN // @DisplayName: Long failsafe action - // @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 + // @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. If FS_LONG_ACTN is set to 2 then instead of using RTL it will enter a FBWA glide with zero throttle. + // @Values: 0:Continue,1:ReturnToLaunch,2:Glide // @User: Standard GSCALAR(long_fs_action, "FS_LONG_ACTN", LONG_FAILSAFE_ACTION), diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde index c3632c133f..43399cb5c7 100644 --- a/ArduPlane/events.pde +++ b/ArduPlane/events.pde @@ -18,16 +18,24 @@ static void failsafe_short_on_event(enum failsafe_state fstype) case TRAINING: failsafe.saved_mode = control_mode; failsafe.saved_mode_set = 1; - set_mode(CIRCLE); + if(g.short_fs_action == 2) { + set_mode(FLY_BY_WIRE_A); + } else { + set_mode(CIRCLE); + } break; case AUTO: case GUIDED: case LOITER: - if(g.short_fs_action == 1) { + if(g.short_fs_action != 0) { failsafe.saved_mode = control_mode; failsafe.saved_mode_set = 1; - set_mode(CIRCLE); + if(g.short_fs_action == 2) { + set_mode(FLY_BY_WIRE_A); + } else { + set_mode(CIRCLE); + } } break; @@ -56,13 +64,19 @@ static void failsafe_long_on_event(enum failsafe_state fstype) case CRUISE: case TRAINING: case CIRCLE: - set_mode(RTL); + if(g.long_fs_action == 2) { + set_mode(FLY_BY_WIRE_A); + } else { + set_mode(RTL); + } break; case AUTO: case GUIDED: case LOITER: - if(g.long_fs_action == 1) { + if(g.long_fs_action == 2) { + set_mode(FLY_BY_WIRE_A); + } else if (g.long_fs_action == 1) { set_mode(RTL); } break; diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 907e52664d..e507a7020a 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -131,12 +131,12 @@ static void control_failsafe(uint16_t pwm) // we detect a failsafe from radio // throttle has dropped below the mark failsafe.ch3_counter++; - if (failsafe.ch3_counter == 9) { + if (failsafe.ch3_counter == 10) { gcs_send_text_fmt(PSTR("MSG FS ON %u"), (unsigned)pwm); - }else if(failsafe.ch3_counter == 10) { failsafe.ch3_failsafe = true; - }else if (failsafe.ch3_counter > 10) { - failsafe.ch3_counter = 11; + } + if (failsafe.ch3_counter > 10) { + failsafe.ch3_counter = 10; } }else if(failsafe.ch3_counter > 0) {