Sub: add params for internal pressure/temperature failsafe actions

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

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;

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;
 

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

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!");