diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 99a229f75d..32b95f0526 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -202,6 +202,34 @@ const AP_Param::Info Sub::var_info[] = { // @User: Standard GSCALAR(failsafe_leak, "FS_LEAK_ENABLE", FS_LEAK_DISABLED), + // @Param: FS_PRESS_ENABLE + // @DisplayName: Internal Pressure Failsafe Enable + // @Description: Controls what action to take if internal pressure exceeds FS_PRESS_MAX parameter. + // @Values: 0:Disabled,1:Warn only + // @User: Standard + GSCALAR(failsafe_pressure, "FS_PRESS_ENABLE", FS_PRESS_DISABLED), + + // @Param: FS_TEMP_ENABLE + // @DisplayName: Internal Temperature Failsafe Enable + // @Description: Controls what action to take if internal temperature exceeds FS_TEMP_MAX parameter. + // @Values: 0:Disabled,1:Warn only + // @User: Standard + GSCALAR(failsafe_temperature, "FS_TEMP_ENABLE", FS_TEMP_DISABLED), + + // @Param: FS_PRESS_MAX + // @DisplayName: Internal Pressure Failsafe Threshold + // @Description: The maximum internal pressure allowed before triggering failsafe. Failsafe action is determined by FS_PRESS_ENABLE parameter + // @Units: Pascal + // @User: Standard + GSCALAR(failsafe_pressure_max, "FS_PRESS_MAX", FS_PRESS_MAX_DEFAULT), + + // @Param: FS_TEMP_MAX + // @DisplayName: Internal Temperature Failsafe Threshold + // @Description: The maximum internal temperature allowed before triggering failsafe. Failsafe action is determined by FS_TEMP_ENABLE parameter. + // @Units: Degrees Centigrade + // @User: Standard + GSCALAR(failsafe_temperature_max, "FS_TEMP_MAX", FS_TEMP_MAX_DEFAULT), + // @Param: GPS_HDOP_GOOD // @DisplayName: GPS Hdop Good // @Description: GPS Hdop value at or below this value represent a good position. Used for pre-arm checks diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index 0af60854ad..68d628e188 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -392,6 +392,10 @@ public: k_param_water_detector, // water detector object k_param_failsafe_leak, // leak failsafe behavior + k_param_failsafe_pressure, // internal pressure failsafe behavior + k_param_failsafe_pressure_max, // maximum internal pressure in pascal before failsafe is triggered + k_param_failsafe_temperature, // internal temperature failsafe behavior + k_param_failsafe_temperature_max // maximum internal temperature in degrees C before failsafe is triggered }; AP_Int16 format_version; @@ -422,6 +426,11 @@ public: AP_Int8 failsafe_leak; // leak detection failsafe behavior AP_Int8 failsafe_gcs; // ground station failsafe behavior + AP_Int8 failsafe_pressure; + AP_Int8 failsafe_temperature; + AP_Int32 failsafe_pressure_max; + AP_Int8 failsafe_temperature_max; + AP_Int16 gps_hdop_good; // GPS Hdop value at or below this value represent a good position AP_Int8 compass_enabled; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index ece072da32..ffab3e488c 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -296,15 +296,15 @@ private: // Failsafe struct { - uint8_t rc_override_active : 1; // 0 // true if rc control are overwritten by ground station - uint8_t radio : 1; // 1 // A status flag for the radio failsafe - uint8_t battery : 1; // 2 // A status flag for the battery failsafe - uint8_t gcs : 1; // 4 // A status flag for the ground station failsafe - uint8_t ekf : 1; // 5 // true if ekf failsafe has occurred - uint8_t terrain : 1; // 6 // true if the missing terrain data failsafe has occurred - uint8_t leak : 1; // true if leak recently detected - uint8_t internal_pressure : 1; - uint8_t internal_temperature :1; + uint8_t rc_override_active : 1; // 0 // true if rc control are overwritten by ground station + uint8_t radio : 1; // 1 // A status flag for the radio failsafe + uint8_t battery : 1; // 2 // A status flag for the battery failsafe + uint8_t gcs : 1; // 4 // A status flag for the ground station failsafe + uint8_t ekf : 1; // 5 // true if ekf failsafe has occurred + uint8_t terrain : 1; // 6 // true if the missing terrain data failsafe has occurred + uint8_t leak : 1; // true if leak recently detected + uint8_t internal_pressure : 1; // true if internal pressure is over threshold + uint8_t internal_temperature : 1; // true if temperature is over threshold uint32_t last_leak_warn_ms; // last time a leak warning was sent to gcs uint32_t last_gcs_warn_ms; diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 34d0d97a0a..83d57abbce 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -466,6 +466,18 @@ enum ThrowModeState { #define FS_LEAK_WARN_ONLY 1 // Only send waring to gcs #define FS_LEAK_SURFACE 2 // Switch to surface mode +// Internal pressure failsafe threshold (FS_PRESS_MAX parameter) +#define FS_PRESS_MAX_DEFAULT 105000 // Maximum internal pressure in pascal before failsafe is triggered +// Internal pressure failsafe definitions (FS_PRESS_ENABLE parameter) +#define FS_PRESS_DISABLED 0 +#define FS_PRESS_WARN_ONLY 1 + +// Internal temperature failsafe threshold (FS_TEMP_MAX parameter) +#define FS_TEMP_MAX_DEFAULT 62 // Maximum internal pressure in degrees C before failsafe is triggered +// Internal temperature failsafe definitions (FS_TEMP_ENABLE parameter) +#define FS_TEMP_DISABLED 0 +#define FS_TEMP_WARN_ONLY 1 + // 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 diff --git a/ArduSub/events.cpp b/ArduSub/events.cpp index fca3933306..f186f2d507 100644 --- a/ArduSub/events.cpp +++ b/ArduSub/events.cpp @@ -1,8 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include "Sub.h" -#define FS_INTERNAL_PRESSURE_MAX 105000 // 103000 pascal -#define FS_INTERNAL_TEMPERATURE_MAX 55 // 50 deg C + /* * This event will be called when the failsafe changes * boolean failsafe reflects the current state @@ -75,20 +74,27 @@ void Sub::failsafe_battery_event(void) } void Sub::failsafe_internal_pressure_check() { + + if(g.failsafe_pressure == FS_PRESS_DISABLED) { + return; // Nothing to do + } + uint32_t tnow = AP_HAL::millis(); static uint32_t last_pressure_warn_ms; static uint32_t last_pressure_good_ms; - if(barometer.get_pressure(0) < FS_INTERNAL_PRESSURE_MAX) { + if(barometer.get_pressure(0) < g.failsafe_pressure_max) { last_pressure_good_ms = tnow; last_pressure_warn_ms = tnow; failsafe.internal_pressure = false; return; } + // 2 seconds with no readings below threshold triggers failsafe if(tnow > last_pressure_good_ms + 2000) { failsafe.internal_pressure = true; } + // Warn every 5 seconds if(failsafe.internal_pressure && tnow > last_pressure_warn_ms + 5000) { last_pressure_warn_ms = tnow; gcs_send_text(MAV_SEVERITY_WARNING, "Internal pressure critical!"); @@ -96,20 +102,27 @@ void Sub::failsafe_internal_pressure_check() { } void Sub::failsafe_internal_temperature_check() { + + if(g.failsafe_temperature == FS_TEMP_DISABLED) { + return; // Nothing to do + } + uint32_t tnow = AP_HAL::millis(); static uint32_t last_temperature_warn_ms; static uint32_t last_temperature_good_ms; - if(barometer.get_temperature(0) < FS_INTERNAL_TEMPERATURE_MAX) { + if(barometer.get_temperature(0) < g.failsafe_temperature_max) { last_temperature_good_ms = tnow; last_temperature_warn_ms = tnow; failsafe.internal_temperature = false; return; } + // 2 seconds with no readings below threshold triggers failsafe if(tnow > last_temperature_good_ms + 2000) { failsafe.internal_temperature = true; } + // Warn every 5 seconds if(failsafe.internal_temperature && tnow > last_temperature_warn_ms + 5000) { last_temperature_warn_ms = tnow; gcs_send_text(MAV_SEVERITY_WARNING, "Internal temperature critical!");