Plane: deploy parachute as fs action

This commit is contained in:
squilter 2016-01-11 10:29:03 -05:00 committed by Andrew Tridgell
parent 24d8610013
commit c7e8a48a55
2 changed files with 14 additions and 4 deletions

View File

@ -558,8 +558,8 @@ const AP_Param::Info Plane::var_info[] = {
// @Param: FS_LONG_ACTN
// @DisplayName: Long failsafe action
// @Description: The action to take on a long (FS_LONG_TIMEOUT seconds) failsafe event. If the aircraft was in a stabilization or manual mode when failsafe started and a long failsafe occurs then it will change to RTL mode if FS_LONG_ACTN is 0 or 1, and will change to FBWA if FS_LONG_ACTN is set to 2. If the aircraft was in an auto mode (such as AUTO or GUIDED) when the failsafe started then it will continue in the auto mode if FS_LONG_ACTN is set to 0, will change to RTL mode if FS_LONG_ACTN is set to 1 and will change to FBWA mode if FS_LONG_ACTN is set to 2.
// @Values: 0:Continue,1:ReturnToLaunch,2:Glide
// @Description: The action to take on a long (FS_LONG_TIMEOUT seconds) failsafe event. If the aircraft was in a stabilization or manual mode when failsafe started and a long failsafe occurs then it will change to RTL mode if FS_LONG_ACTN is 0 or 1, and will change to FBWA if FS_LONG_ACTN is set to 2. If the aircraft was in an auto mode (such as AUTO or GUIDED) when the failsafe started then it will continue in the auto mode if FS_LONG_ACTN is set to 0, will change to RTL mode if FS_LONG_ACTN is set to 1 and will change to FBWA mode if FS_LONG_ACTN is set to 2. If FS_LONG_ACTION is set to 3, the parachute will be deployed (make sure the chute is configured and enabled).
// @Values: 0:Continue,1:ReturnToLaunch,2:Glide,3:Deploy Parachute
// @User: Standard
GSCALAR(long_fs_action, "FS_LONG_ACTN", LONG_FAILSAFE_ACTION),

View File

@ -75,7 +75,12 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
case CRUISE:
case TRAINING:
case CIRCLE:
if(g.long_fs_action == 2) {
if(g.long_fs_action == 3) {
#if PARACHUTE == ENABLED
parachute_release();
disarm_motors();
#endif
} else if (g.long_fs_action == 2) {
set_mode(FLY_BY_WIRE_A);
} else {
set_mode(RTL);
@ -90,7 +95,12 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
case AUTO:
case GUIDED:
case LOITER:
if(g.long_fs_action == 2) {
if(g.long_fs_action == 3) {
#if PARACHUTE == ENABLED
parachute_release();
disarm_motors();
#endif
} else if (g.long_fs_action == 2) {
set_mode(FLY_BY_WIRE_A);
} else if (g.long_fs_action == 1) {
set_mode(RTL);