Rover: add continue-in-auto option to failsafe

This commit is contained in:
Tatsuya Yamaguchi 2018-11-13 17:40:40 +09:00 committed by Randy Mackay
parent 85243ad522
commit 32ba248c73
3 changed files with 25 additions and 5 deletions

View File

@ -144,9 +144,9 @@ const AP_Param::Info Rover::var_info[] = {
// @Param: FS_THR_ENABLE // @Param: FS_THR_ENABLE
// @DisplayName: Throttle Failsafe Enable // @DisplayName: Throttle Failsafe Enable
// @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel to a low value. This can be used to detect the RC transmitter going out of range. Failsafe will be triggered when the throttle channel goes below the FS_THR_VALUE for FS_TIMEOUT seconds. // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel to a low value. This can be used to detect the RC transmitter going out of range. Failsafe will be triggered when the throttle channel goes below the FS_THR_VALUE for FS_TIMEOUT seconds.
// @Values: 0:Disabled,1:Enabled // @Values: 0:Disabled,1:Enabled,2:Enabled Continue with Mission in Auto
// @User: Standard // @User: Standard
GSCALAR(fs_throttle_enabled, "FS_THR_ENABLE", 1), GSCALAR(fs_throttle_enabled, "FS_THR_ENABLE", FS_THR_ENABLED),
// @Param: FS_THR_VALUE // @Param: FS_THR_VALUE
// @DisplayName: Throttle Failsafe Value // @DisplayName: Throttle Failsafe Value
@ -159,9 +159,9 @@ const AP_Param::Info Rover::var_info[] = {
// @Param: FS_GCS_ENABLE // @Param: FS_GCS_ENABLE
// @DisplayName: GCS failsafe enable // @DisplayName: GCS failsafe enable
// @Description: Enable ground control station telemetry failsafe. When enabled the Rover will execute the FS_ACTION when it fails to receive MAVLink heartbeat packets for FS_TIMEOUT seconds. // @Description: Enable ground control station telemetry failsafe. When enabled the Rover will execute the FS_ACTION when it fails to receive MAVLink heartbeat packets for FS_TIMEOUT seconds.
// @Values: 0:Disabled,1:Enabled // @Values: 0:Disabled,1:Enabled,2:Enabled Continue with Mission in Auto
// @User: Standard // @User: Standard
GSCALAR(fs_gcs_enabled, "FS_GCS_ENABLE", 0), GSCALAR(fs_gcs_enabled, "FS_GCS_ENABLE", FS_GCS_DISABLED),
// @Param: FS_CRASH_CHECK // @Param: FS_CRASH_CHECK
// @DisplayName: Crash check action // @DisplayName: Crash check action

View File

@ -77,6 +77,20 @@
// subsystem specific error codes -- crash checker // subsystem specific error codes -- crash checker
#define ERROR_CODE_CRASH_CHECK_CRASH 1 #define ERROR_CODE_CRASH_CHECK_CRASH 1
// radio failsafe enum (FS_THR_ENABLE parameter)
enum fs_thr_enable {
FS_THR_DISABLED = 0,
FS_THR_ENABLED,
FS_THR_ENABLED_CONTINUE_MISSION,
};
// gcs failsafe enum (FS_GCS_ENABLE parameter)
enum fs_gcs_enable {
FS_GCS_DISABLED = 0,
FS_GCS_ENABLED,
FS_GCS_ENABLED_CONTINUE_MISSION,
};
enum fs_crash_action { enum fs_crash_action {
FS_CRASH_DISABLE = 0, FS_CRASH_DISABLE = 0,
FS_CRASH_HOLD = 1, FS_CRASH_HOLD = 1,

View File

@ -73,6 +73,11 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
// clear rc overrides // clear rc overrides
RC_Channels::clear_overrides(); RC_Channels::clear_overrides();
if ((control_mode == &mode_auto) &&
((failsafe_type == FAILSAFE_EVENT_THROTTLE && g.fs_throttle_enabled == FS_THR_ENABLED_CONTINUE_MISSION) ||
(failsafe_type == FAILSAFE_EVENT_GCS && g.fs_gcs_enabled == FS_GCS_ENABLED_CONTINUE_MISSION))) {
// continue with mission in auto mode
} else {
switch (g.fs_action) { switch (g.fs_action) {
case Failsafe_Action_None: case Failsafe_Action_None:
break; break;
@ -98,6 +103,7 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
break; break;
} }
} }
}
} }
void Rover::handle_battery_failsafe(const char* type_str, const int8_t action) void Rover::handle_battery_failsafe(const char* type_str, const int8_t action)