Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Rework pilot input failsafe, add enable and timeout params

Browse Source
This commit is contained in:
Jacob Walser 2017-04-14 17:23:59 -04:00
parent 1ff656df2d
commit 785f774887
4 changed files with 44 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
ArduSub/Parameters.cpp
View File

@ -165,6 +165,21 @@ const AP_Param::Info Sub::var_info[] = {
// @User: Standard
GSCALAR(failsafe_terrain, "FS_TERRAIN_ENAB", FS_TERRAIN_DISARM),
// @Param: FS_PILOT_INPUT
// @DisplayName: Pilot input failsafe action
// @Description: Controls what action to take if no pilot input has been received after the timeout period specified by the FS_PILOT_TIMEOUT parameter
// @Values: 0:Disabled, 1:Warn Only, 2:Disarm
// @User: Standard
GSCALAR(failsafe_pilot_input, "FS_PILOT_INPUT", FS_PILOT_INPUT_DISARM),
// @Param: FS_PILOT_TIMEOUT
// @DisplayName: Timeout for activation of pilot input failsafe
// @Description: Controls the maximum interval between received pilot inputs before the failsafe action is triggered
// @Units: Seconds
// @Range 0.1 3.0
// @User: Standard
GSCALAR(failsafe_pilot_input_timeout, "FS_PILOT_TIMEOUT", 1.0f),
// @Param: XTRACK_ANG_LIM
// @DisplayName: Crosstrack correction angle limit
// @Description: Maximum allowed angle (in degrees) between current track and desired heading during waypoint navigation

4
ArduSub/Parameters.h
View File

@ -170,6 +170,8 @@ public:
k_param_failsafe_battery_enabled,
k_param_fs_batt_mah,
k_param_fs_batt_voltage,
k_param_failsafe_pilot_input,
k_param_failsafe_pilot_input_timeout,
// Misc Sub settings
@ -233,6 +235,8 @@ public:
AP_Int32 failsafe_pressure_max;
AP_Int8 failsafe_temperature_max;
AP_Int8 failsafe_terrain;
AP_Int8 failsafe_pilot_input; // pilot input failsafe behavior
AP_Float failsafe_pilot_input_timeout;
AP_Int8 xtrack_angle_limit;

5
ArduSub/defines.h
View File

@ -303,6 +303,11 @@ enum RTLState {
#define FS_TERRAIN_HOLD 1
#define FS_TERRAIN_SURFACE 2
// Pilot input failsafe actions
#define FS_PILOT_INPUT_DISABLED 0
#define FS_PILOT_INPUT_WARN_ONLY 1
#define FS_PILOT_INPUT_DISARM 2
// Amount of time to attempt recovery of valid rangefinder data before
// initiating terrain failsafe action
#define FS_TERRAIN_RECOVER_TIMEOUT_MS 10000

32
ArduSub/failsafe.cpp
View File

@ -168,21 +168,29 @@ void Sub::failsafe_battery_check(void)
void Sub::failsafe_pilot_input_check()
{
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL
uint32_t tnow = AP_HAL::millis();
// Require at least 0.5 Hz update
if (tnow > failsafe.last_pilot_input_ms + 2000) {
if (!failsafe.pilot_input) {
failsafe.pilot_input = true;
set_neutral_controls();
init_disarm_motors();
Log_Write_Error(ERROR_SUBSYSTEM_INPUT, ERROR_CODE_FAILSAFE_OCCURRED);
gcs_send_text(MAV_SEVERITY_CRITICAL, "Lost manual control");
}
if (g.failsafe_pilot_input == FS_PILOT_INPUT_DISABLED) {
failsafe.pilot_input = false;
return;
}
failsafe.pilot_input = false;
if (AP_HAL::millis() < failsafe.last_pilot_input_ms + g.failsafe_pilot_input_timeout * 1000.0f) {
failsafe.pilot_input = false; // We've received an update from the pilot within the timeout period
return;
}
if (failsafe.pilot_input) {
return; // only act once
}
failsafe.pilot_input = true;
Log_Write_Error(ERROR_SUBSYSTEM_INPUT, ERROR_CODE_FAILSAFE_OCCURRED);
gcs_send_text(MAV_SEVERITY_CRITICAL, "Lost manual control");
if(g.failsafe_pilot_input == FS_PILOT_INPUT_DISARM) {
set_neutral_controls();
init_disarm_motors();
}
#endif
}