Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 20:48:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: add params for internal pressure/temperature failsafe actions

Browse Source
This commit is contained in:
Jacob Walser 2016-09-11 17:49:19 -04:00 committed by Andrew Tridgell
parent 70d58cd0bc
commit 88e234272f
5 changed files with 75 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
ArduSub/Parameters.cpp
View File

@ -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

9
ArduSub/Parameters.h
View File

@ -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;

18
ArduSub/Sub.h
View File

@ -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;

12
ArduSub/defines.h
View File

@ -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

21
ArduSub/events.cpp
View File

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