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

Sub: Add GCS warnings for high internal pressure/temperature

Browse Source
This commit is contained in:
Jacob Walser 2016-09-03 18:17:34 -04:00 committed by Andrew Tridgell
parent 8f6b3900a3
commit 1143f98fad
3 changed files with 52 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduSub/ArduSub.cpp
View File

@ -462,6 +462,10 @@ void Sub::three_hz_loop()
{
set_leak_status(water_detector.update());
failsafe_internal_pressure_check();
failsafe_internal_temperature_check();
// check if we've lost contact with the ground station
failsafe_gcs_check();

4
ArduSub/Sub.h
View File

@ -303,6 +303,8 @@ private:
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;
uint32_t last_leak_warn_ms; // last time a leak warning was sent to gcs
uint32_t last_gcs_warn_ms;
@ -1043,6 +1045,8 @@ private:
void accel_cal_update(void);
void set_leak_status(bool status);
void failsafe_internal_pressure_check();
void failsafe_internal_temperature_check();
bool surface_init(bool ignore_flags);
void surface_run();

45
ArduSub/events.cpp
View File

@ -1,7 +1,8 @@
// -*- 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
@ -73,6 +74,48 @@ void Sub::failsafe_battery_event(void)
}
void Sub::failsafe_internal_pressure_check() {
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) {
last_pressure_good_ms = tnow;
last_pressure_warn_ms = tnow;
failsafe.internal_pressure = false;
return;
}
if(tnow > last_pressure_good_ms + 2000) {
failsafe.internal_pressure = true;
}
if(failsafe.internal_pressure && tnow > last_pressure_warn_ms + 5000) {
last_pressure_warn_ms = tnow;
gcs_send_text(MAV_SEVERITY_WARNING, "Internal pressure critical!");
}
}
void Sub::failsafe_internal_temperature_check() {
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) {
last_temperature_good_ms = tnow;
last_temperature_warn_ms = tnow;
failsafe.internal_temperature = false;
return;
}
if(tnow > last_temperature_good_ms + 2000) {
failsafe.internal_temperature = true;
}
if(failsafe.internal_temperature && tnow > last_temperature_warn_ms + 5000) {
last_temperature_warn_ms = tnow;
gcs_send_text(MAV_SEVERITY_WARNING, "Internal temperature critical!");
}
}
void Sub::set_leak_status(bool status) {
AP_Notify::flags.leak_detected = status;