aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Sub: Add leak and gcs failsafe actions

Browse Source
This commit is contained in:
Jacob Walser 2016-08-26 15:27:28 -04:00 committed by Andrew Tridgell
parent e2a7ba5b0c
commit 5911785455
6 changed files with 78 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduSub/AP_State.cpp
View File

@ -91,12 +91,6 @@ void Sub::set_failsafe_battery(bool b)
AP_Notify::flags.failsafe_battery = b;
}
// ---------------------------------------------
void Sub::set_failsafe_gcs(bool b)
{
failsafe.gcs = b;
}
// ---------------------------------------------
void Sub::set_pre_arm_check(bool b)

13
ArduSub/Parameters.cpp
View File

@ -190,10 +190,17 @@ const AP_Param::Info Sub::var_info[] = {
// @Param: FS_GCS_ENABLE
// @DisplayName: Ground Station Failsafe Enable
// @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds. NB. The GCS Failsafe is only active when RC_OVERRIDE is being used to control the vehicle.
// @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode
// @Description: Controls what action to take when GCS heartbeat is lost.
// @Values: 0:Disabled,1:Warn only,2:Disarm,3:Enter depth hold mode,4:Enter surface mode
// @User: Standard
GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_ENABLED_ALWAYS_RTL),
GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_DISABLED),
// @Param: FS_LEAK_ENABLE
// @DisplayName: Leak Failsafe Enable
// @Description: Controls what action to take if a leak is detected.
// @Values: 0:Disabled,1:Warn only,2:Enter surface mode
// @User: Standard
GSCALAR(failsafe_leak, "FS_LEAK_ENABLE", FS_LEAK_DISABLED),
// @Param: GPS_HDOP_GOOD
// @DisplayName: GPS Hdop Good

2
ArduSub/Parameters.h
View File

@ -388,6 +388,7 @@ public:
k_param_jbtn_15, // 276
k_param_water_detector, // water detector object
k_param_failsafe_leak, // leak failsafe behavior
};
AP_Int16 format_version;
@ -416,6 +417,7 @@ public:
AP_Float fs_batt_voltage; // battery voltage below which failsafe will be triggered
AP_Float fs_batt_mah; // battery capacity (in mah) below which failsafe will be triggered
AP_Int8 failsafe_leak; // leak detection failsafe behavior
AP_Int8 failsafe_gcs; // ground station failsafe behavior
AP_Int16 gps_hdop_good; // GPS Hdop value at or below this value represent a good position

3
ArduSub/Sub.h
View File

@ -304,6 +304,7 @@ private:
uint8_t terrain : 1; // 6 // true if the missing terrain data failsafe has occurred
uint8_t leak : 1; // true if leak recently detected
uint32_t last_leak_warn_ms; // last time a leak warning was sent to gcs
uint32_t last_gcs_warn_ms;
int8_t radio_counter; // number of iterations with throttle below throttle_fs_value
@ -603,7 +604,6 @@ private:
void set_simple_mode(uint8_t b);
void set_failsafe_radio(bool b);
void set_failsafe_battery(bool b);
void set_failsafe_gcs(bool b);
void set_land_complete(bool b);
void set_land_complete_maybe(bool b);
void set_pre_arm_check(bool b);
@ -848,7 +848,6 @@ private:
void failsafe_radio_off_event();
void failsafe_battery_event(void);
void failsafe_gcs_check();
void failsafe_gcs_off_event(void);
void failsafe_terrain_check();
void failsafe_terrain_set_status(bool data_ok);
void failsafe_terrain_on_event();

18
ArduSub/defines.h
View File

@ -132,7 +132,8 @@ enum mode_reason_t {
MODE_REASON_TERRAIN_FAILSAFE,
MODE_REASON_BRAKE_TIMEOUT,
MODE_REASON_FLIP_COMPLETE,
MODE_REASON_SURFACE_COMPLETE
MODE_REASON_SURFACE_COMPLETE,
MODE_REASON_LEAK_FAILSAFE
};
// Tuning enumeration
@ -454,11 +455,18 @@ enum ThrowModeState {
#define FS_BATT_RTL 2 // switch to RTL mode on battery failsafe
// GCS failsafe definitions (FS_GCS_ENABLE parameter)
#define FS_GCS_DISABLED 0
#define FS_GCS_ENABLED_ALWAYS_RTL 1
#define FS_GCS_ENABLED_CONTINUE_MISSION 2
#define FS_GCS_DISABLED 0 // Disabled
#define FS_GCS_WARN_ONLY 1 // Only send warning to gcs (only useful with multiple gcs links)
#define FS_GCS_DISARM 2 // Disarm
#define FS_GCS_HOLD 3 // Switch depth hold mode or poshold mode if available
#define FS_GCS_SURFACE 4 // Switch to surface mode
// EKF failsafe definitions (FS_EKF_ACTION parameter)
// Leak failsafe definitions (FS_LEAK_ENABLE parameter)
#define FS_LEAK_DISABLED 0 // Disabled
#define FS_LEAK_WARN_ONLY 1 // Only send waring to gcs
#define FS_LEAK_SURFACE 2 // Switch to surface mode
// EKF failsafe definitions (FS_EKF_ENABLE parameter)
#define FS_EKF_ACTION_LAND 1 // switch to LAND mode on EKF failsafe
#define FS_EKF_ACTION_ALTHOLD 2 // switch to ALTHOLD mode on EKF failsafe
#define FS_EKF_ACTION_LAND_EVEN_STABILIZE 3 // switch to Land mode on EKF failsafe even if in a manual flight mode like stabilize

85
ArduSub/events.cpp
View File

@ -74,65 +74,84 @@ void Sub::failsafe_battery_event(void)
}
void Sub::set_leak_status(bool status) {
uint32_t tnow = AP_HAL::millis();
failsafe.leak = status;
AP_Notify::flags.leak_detected = status;
if(failsafe.leak && tnow > failsafe.last_leak_warn_ms + 5000) {
// Do nothing if we are dry, or if leak failsafe action is disabled
if(status == false || g.failsafe_leak == FS_LEAK_DISABLED) {
failsafe.leak = false;
return;
}
uint32_t tnow = AP_HAL::millis();
// We have a leak
// Always send a warning every 5 seconds
if(tnow > failsafe.last_leak_warn_ms + 5000) {
failsafe.last_leak_warn_ms = tnow;
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Leak Detected");
gcs_send_text(MAV_SEVERITY_WARNING, "Leak Detected");
}
// Do nothing if we have already triggered the failsafe action, or if the motors are disarmed
if(failsafe.leak || !motors.armed()) {
return;
}
failsafe.leak = true;
// Handle failsafe action
if(g.failsafe_leak == FS_LEAK_SURFACE && motors.armed()) {
set_mode(SURFACE, MODE_REASON_LEAK_FAILSAFE);
}
}
// failsafe_gcs_check - check for ground station failsafe
void Sub::failsafe_gcs_check()
{
uint32_t last_gcs_update_ms;
// return immediately if gcs failsafe is disabled, gcs has never been connected or we are not overriding rc controls from the gcs and we are not in guided mode
// return immediately if gcs failsafe action is disabled
// this also checks to see if we have a GCS failsafe active, if we do, then must continue to process the logic for recovery from this state.
if ((!failsafe.gcs) && (g.failsafe_gcs == FS_GCS_DISABLED)) {
if (!g.failsafe_gcs && g.failsafe_gcs == FS_GCS_DISABLED) {
return;
}
// calc time since last gcs update
// note: this only looks at the heartbeat from the device id set by g.sysid_my_gcs
last_gcs_update_ms = AP_HAL::millis() - failsafe.last_heartbeat_ms;
uint32_t tnow = AP_HAL::millis();
// check if all is well
if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS) {
// check for recovery from gcs failsafe
// Check if we have gotten a GCS heartbeat recently (GCS sysid must match SYSID_MYGCS parameter)
if (tnow < failsafe.last_heartbeat_ms + FS_GCS_TIMEOUT_MS) {
// Log event if we are recovering from previous gcs failsafe
if (failsafe.gcs) {
failsafe_gcs_off_event();
set_failsafe_gcs(false);
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED);
}
failsafe.gcs = false;
return;
}
// do nothing if gcs failsafe already triggered or motors disarmed
//////////////////////////////
// GCS heartbeat has timed out
//////////////////////////////
// Send a warning every 5 seconds
if(tnow > failsafe.last_gcs_warn_ms + 5000) {
failsafe.last_gcs_warn_ms = tnow;
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "MYGCS: %d, heartbeat lost", g.sysid_my_gcs);
}
// do nothing if we have already triggered the failsafe action, or if the motors are disarmed
if (failsafe.gcs || !motors.armed()) {
return;
}
// GCS failsafe event has occured
// update state, log to dataflash
set_failsafe_gcs(true);
failsafe.gcs = true;
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_OCCURRED);
// clear overrides so that RC control can be regained with radio.
hal.rcin->clear_overrides();
failsafe.rc_override_active = false;
// Disarm the motors, pilot has lost all contact/control over vehicle.
init_disarm_motors();
}
// failsafe_gcs_off_event - actions to take when GCS contact is restored
void Sub::failsafe_gcs_off_event(void)
{
// log recovery of GCS in logs?
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED);
// handle failsafe action
if(g.failsafe_gcs == FS_GCS_DISARM) {
init_disarm_motors();
} else if (g.failsafe_gcs == FS_GCS_HOLD && motors.armed()) {
set_mode(ALT_HOLD, MODE_REASON_GCS_FAILSAFE);
} else if (g.failsafe_gcs == FS_GCS_SURFACE && motors.armed()) {
set_mode(SURFACE, MODE_REASON_GCS_FAILSAFE);
}
}
// executes terrain failsafe if data is missing for longer than a few seconds