Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: Add failsafe mechanisms for depth sensor error

This commit is contained in:
Jacob Walser 2017-04-15 01:03:56 -04:00
parent 785f774887
commit c093e1c37e
10 changed files with 55 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduSub/ArduSub.cpp
View File

@ -216,6 +216,8 @@ void Sub::fifty_hz_loop()
failsafe_ekf_check();
failsafe_sensors_check();
// Update servo output
RC_Channels::set_pwm_all();
SRV_Channels::limit_slew_rate(SRV_Channel::k_mount_tilt, g.cam_slew_limit, 0.02f);

6
ArduSub/GCS_Mavlink.cpp
View File

@ -156,9 +156,7 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan)
MAV_SYS_STATUS_SENSOR_GPS |
MAV_SYS_STATUS_SENSOR_RC_RECEIVER);
if (ap.depth_sensor_present && barometer.all_healthy()) { // check all barometers
control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
} else if (barometer.healthy(0)) { // check the internal barometer only
if (sensor_health.depth) { // check the internal barometer only
control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
}
if (g.compass_enabled && compass.healthy() && ahrs.use_compass()) {
@ -1259,7 +1257,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
result = MAV_RESULT_FAILED;
}
} else if (is_equal(packet.param3,1.0f)) {
if (!sub.ap.depth_sensor_present || sub.motors.armed() || sub.barometer.get_pressure() > 110000) {
if (!sub.sensor_health.depth || sub.motors.armed() || sub.barometer.get_pressure() > 110000) {
result = MAV_RESULT_FAILED;
} else {
sub.init_barometer(true);

11
ArduSub/Sub.h
View File

@ -237,7 +237,6 @@ private:
enum HomeState home_state : 2; // home status (unset, set, locked)
uint8_t at_bottom : 1; // true if we are at the bottom
uint8_t at_surface : 1; // true if we are at the surface
uint8_t depth_sensor_present: 1; // true if we have an external baro connected
};
uint32_t value;
} ap;
@ -268,9 +267,10 @@ private:
uint8_t internal_pressure : 1; // true if internal pressure is over threshold
uint8_t internal_temperature : 1; // true if temperature is over threshold
uint8_t crash : 1; // true if we are crashed
uint8_t sensor_health : 1; // true if at least one sensor has triggered a failsafe (currently only used for depth in depth enabled modes)
uint32_t last_leak_warn_ms; // last time a leak warning was sent to gcs
uint32_t last_gcs_warn_ms;
uint32_t last_heartbeat_ms; // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe
uint32_t last_pilot_input_ms; // last time we received pilot input in the form of MANUAL_CONTROL or RC_CHANNELS_OVERRIDE messages
uint32_t terrain_first_failure_ms; // the first time terrain data access failed - used to calculate the duration of the failure
@ -282,10 +282,14 @@ private:
// sensor health for logging
struct {
uint8_t baro : 1; // true if baro is healthy
uint8_t baro : 1; // true if any single baro is healthy
uint8_t depth : 1; // true if depth sensor is healthy
uint8_t compass : 1; // true if compass is healthy
} sensor_health;
// Baro sensor instance index of the external water pressure sensor
uint8_t depth_sensor_idx;
AP_Motors6DOF motors;
// GPS variables
@ -590,6 +594,7 @@ private:
void stabilize_run();
bool manual_init(void);
void manual_run();
void failsafe_sensors_check(void);
void failsafe_crash_check();
void failsafe_ekf_check(void);
void failsafe_battery_check(void);

4
ArduSub/control_althold.cpp
View File

@ -9,8 +9,8 @@
bool Sub::althold_init()
{
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL
if (!ap.depth_sensor_present) { // can't hold depth without a depth sensor, exit immediately.
gcs_send_text(MAV_SEVERITY_WARNING, "Depth hold requires external pressure sensor.");
if (!sensor_health.depth) { // can't hold depth without a depth sensor, exit immediately.
gcs_send_text(MAV_SEVERITY_WARNING, "BAD DEPTH");
return false;
}
#endif

4
ArduSub/control_surface.cpp
View File

@ -4,8 +4,8 @@
bool Sub::surface_init()
{
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL
if (!ap.depth_sensor_present) { // can't hold depth without a depth sensor, exit immediately.
gcs_send_text(MAV_SEVERITY_WARNING, "Surface mode requires external pressure sensor.");
if (!sensor_health.depth) { // can't hold depth without a depth sensor, exit immediately.
gcs_send_text(MAV_SEVERITY_WARNING, "BAD DEPTH");
return false;
}
#endif

8
ArduSub/defines.h
View File

@ -56,7 +56,8 @@ enum mode_reason_t {
MODE_REASON_FENCE_BREACH,
MODE_REASON_TERRAIN_FAILSAFE,
MODE_REASON_SURFACE_COMPLETE,
MODE_REASON_LEAK_FAILSAFE
MODE_REASON_LEAK_FAILSAFE,
MODE_REASON_BAD_DEPTH
};
// Acro Trainer types
@ -193,6 +194,8 @@ enum RTLState {
#define ERROR_SUBSYSTEM_NAVIGATION 22
#define ERROR_SUBSYSTEM_FAILSAFE_TERRAIN 23
#define ERROR_SUBSYSTEM_FAILSAFE_LEAK 24
#define ERROR_SUBSYSTEM_FAILSAFE_SENSORS 25
// general error codes
#define ERROR_CODE_ERROR_RESOLVED 0
#define ERROR_CODE_FAILED_TO_INITIALISE 1
@ -220,8 +223,9 @@ enum RTLState {
// EKF check definitions
#define ERROR_CODE_EKFCHECK_BAD_VARIANCE 2
#define ERROR_CODE_EKFCHECK_VARIANCE_CLEARED 0
// Baro specific error codes
#define ERROR_CODE_BARO_GLITCH 2
#define ERROR_CODE_BAD_DEPTH 0
//////////////////////////////////////////////////////////////////////////////
// Battery monitoring

26
ArduSub/failsafe.cpp
View File

@ -64,6 +64,32 @@ void Sub::mainloop_failsafe_check()
}
}
void Sub::failsafe_sensors_check(void)
{
// We need a depth sensor to do any sort of auto z control
if (sensor_health.depth) {
failsafe.sensor_health = false;
return;
}
// only report once
if (failsafe.sensor_health) {
return;
}
failsafe.sensor_health = true;
gcs_send_text(MAV_SEVERITY_CRITICAL, "Depth sensor error!");
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_SENSORS, ERROR_CODE_BAD_DEPTH);
if (control_mode == ALT_HOLD || control_mode == SURFACE || mode_requires_GPS(control_mode)) {
// This should always succeed
if (!set_mode(MANUAL, MODE_REASON_BAD_DEPTH)) {
// We should never get here
init_disarm_motors();
}
}
}
void Sub::failsafe_ekf_check(void)
{
static uint32_t last_ekf_good_ms = 0;

2
ArduSub/sensors.cpp
View File

@ -18,6 +18,8 @@ void Sub::read_barometer(void)
baro_climbrate = barometer.get_climb_rate() * 100.0f;
motors.set_air_density_ratio(barometer.get_air_density_ratio());
sensor_health.depth = barometer.healthy(depth_sensor_idx);
}
void Sub::init_rangefinder(void)

2
ArduSub/surface_bottom_detector.cpp
View File

@ -24,7 +24,7 @@ void Sub::update_surface_and_bottom_detector()
// check that we are not moving up or down
bool vel_stationary = velocity.z > -0.05 && velocity.z < 0.05;
if (ap.depth_sensor_present) { // we can use the external pressure sensor for a very accurate and current measure of our z axis position
if (sensor_health.depth) { // we can use the external pressure sensor for a very accurate and current measure of our z axis position
current_depth = barometer.get_altitude(); // cm

7
ArduSub/system.cpp
View File

@ -134,14 +134,15 @@ void Sub::init_ardupilot()
barometer.update();
for (uint8_t i = 0; i < barometer.num_instances(); i++) {
if (barometer.get_type(i) == AP_Baro::BARO_TYPE_WATER && barometer.healthy(i)) {
if (barometer.get_type(i) == AP_Baro::BARO_TYPE_WATER) {
barometer.set_primary_baro(i);
ap.depth_sensor_present = true;
depth_sensor_idx = i;
sensor_health.depth = barometer.healthy(i);
break;
}
}
if (!ap.depth_sensor_present) {
if (!sensor_health.depth) {
// We only have onboard baro
// No external underwater depth sensor detected
barometer.set_primary_baro(0);