From bb2215bb9f2d7034d6ca4a1b79dddca43929313b Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Thu, 23 Mar 2017 15:06:46 -0400 Subject: [PATCH] Sub: Move failsafe functions into failsafe.cpp --- ArduSub/events.cpp | 296 ------------------------------------------- ArduSub/failsafe.cpp | 296 +++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 296 insertions(+), 296 deletions(-) delete mode 100644 ArduSub/events.cpp diff --git a/ArduSub/events.cpp b/ArduSub/events.cpp deleted file mode 100644 index 8dbc0f31c3..0000000000 --- a/ArduSub/events.cpp +++ /dev/null @@ -1,296 +0,0 @@ -#include "Sub.h" - -void Sub::failsafe_battery_event(void) -{ - // // return immediately if low battery event has already been triggered - // if (failsafe.battery) { - // return; - // } - // - // // failsafe check - // if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors.armed()) { - // if (should_disarm_on_failsafe()) { - // init_disarm_motors(); - // } else { - // if (g.failsafe_battery_enabled == FS_BATT_RTL || control_mode == AUTO) { - // set_mode_RTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE); - // } else { - // set_mode_land_with_pause(MODE_REASON_BATTERY_FAILSAFE); - // } - // } - // } - // - // // set the low battery flag - // set_failsafe_battery(true); - // - // // warn the ground station and log to dataflash - // gcs_send_text(MAV_SEVERITY_WARNING,"Low battery"); - // Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED); - -} - -void Sub::failsafe_manual_control_check() -{ -#if CONFIG_HAL_BOARD != HAL_BOARD_SITL - uint32_t tnow = AP_HAL::millis(); - - // Require at least 0.5 Hz update - if (tnow > failsafe.last_manual_control_ms + 2000) { - if (!failsafe.manual_control) { - failsafe.manual_control = true; - set_neutral_controls(); - init_disarm_motors(); - Log_Write_Error(ERROR_SUBSYSTEM_INPUT, ERROR_CODE_FAILSAFE_OCCURRED); - gcs_send_text(MAV_SEVERITY_CRITICAL, "Lost manual control"); - } - return; - } - - failsafe.manual_control = false; -#endif -} - -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) < 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 30 seconds - if (failsafe.internal_pressure && tnow > last_pressure_warn_ms + 30000) { - last_pressure_warn_ms = tnow; - gcs_send_text(MAV_SEVERITY_WARNING, "Internal pressure critical!"); - } -} - -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) < 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 30 seconds - if (failsafe.internal_temperature && tnow > last_temperature_warn_ms + 30000) { - 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; - - // Do nothing if we are dry, or if leak failsafe action is disabled - if (status == false || g.failsafe_leak == FS_LEAK_DISABLED) { - if (failsafe.leak) { - Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_LEAK, ERROR_CODE_FAILSAFE_RESOLVED); - } - failsafe.leak = false; - return; - } - - uint32_t tnow = AP_HAL::millis(); - - // We have a leak - // Always send a warning every 20 seconds - if (tnow > failsafe.last_leak_warn_ms + 20000) { - failsafe.last_leak_warn_ms = tnow; - gcs_send_text(MAV_SEVERITY_CRITICAL, "Leak Detected"); - } - - // Do nothing if we have already triggered the failsafe action, or if the motors are disarmed - if (failsafe.leak) { - return; - } - - failsafe.leak = true; - - Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_LEAK, ERROR_CODE_FAILSAFE_OCCURRED); - - // Handle failsafe action - if (failsafe.leak && g.failsafe_leak == FS_LEAK_SURFACE && motors.armed()) { - set_mode(SURFACE, MODE_REASON_LEAK_FAILSAFE); - } -} - -// failsafe_gcs_check - check for ground station failsafe -void Sub::failsafe_gcs_check() -{ - // return immediately if we have never had contact with a gcs, or if gcs failsafe action is disabled - // this also checks to see if we have a GCS failsafe active, if we do, then must continue to process the logic for recovery from this state. - if (failsafe.last_heartbeat_ms == 0 || (!g.failsafe_gcs && g.failsafe_gcs == FS_GCS_DISABLED)) { - return; - } - - uint32_t tnow = AP_HAL::millis(); - - // Check if we have gotten a GCS heartbeat recently (GCS sysid must match SYSID_MYGCS parameter) - if (tnow < failsafe.last_heartbeat_ms + FS_GCS_TIMEOUT_MS) { - // Log event if we are recovering from previous gcs failsafe - if (failsafe.gcs) { - Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED); - } - failsafe.gcs = false; - return; - } - - ////////////////////////////// - // GCS heartbeat has timed out - ////////////////////////////// - - // Send a warning every 30 seconds - if (tnow > failsafe.last_gcs_warn_ms + 30000) { - failsafe.last_gcs_warn_ms = tnow; - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "MYGCS: %d, heartbeat lost", g.sysid_my_gcs); - } - - // do nothing if we have already triggered the failsafe action, or if the motors are disarmed - if (failsafe.gcs || !motors.armed()) { - return; - } - - // update state, log to dataflash - failsafe.gcs = true; - Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_OCCURRED); - - // handle failsafe action - if (g.failsafe_gcs == FS_GCS_DISARM) { - init_disarm_motors(); - } else if (g.failsafe_gcs == FS_GCS_HOLD && motors.armed()) { - set_mode(ALT_HOLD, MODE_REASON_GCS_FAILSAFE); - } else if (g.failsafe_gcs == FS_GCS_SURFACE && motors.armed()) { - set_mode(SURFACE, MODE_REASON_GCS_FAILSAFE); - } -} - -// executes terrain failsafe if data is missing for longer than a few seconds -// missing_data should be set to true if the vehicle failed to navigate because of missing data, false if navigation is proceeding successfully -void Sub::failsafe_terrain_check() -{ - // trigger with 5 seconds of failures while in AUTO mode - bool valid_mode = (control_mode == AUTO || control_mode == GUIDED); - bool timeout = (failsafe.terrain_last_failure_ms - failsafe.terrain_first_failure_ms) > FS_TERRAIN_TIMEOUT_MS; - bool trigger_event = valid_mode && timeout; - - // check for clearing of event - if (trigger_event != failsafe.terrain) { - if (trigger_event) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Failsafe terrain triggered"); - failsafe_terrain_on_event(); - } else { - Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_ERROR_RESOLVED); - failsafe.terrain = false; - } - } -} - -// This gets called if mission items are in ALT_ABOVE_TERRAIN frame -// Terrain failure occurs when terrain data is not found, or rangefinder is not enabled or healthy -// set terrain data status (found or not found) -void Sub::failsafe_terrain_set_status(bool data_ok) -{ - uint32_t now = millis(); - - // record time of first and latest failures (i.e. duration of failures) - if (!data_ok) { - failsafe.terrain_last_failure_ms = now; - if (failsafe.terrain_first_failure_ms == 0) { - failsafe.terrain_first_failure_ms = now; - } - } else { - // failures cleared after 0.1 seconds of persistent successes - if (now - failsafe.terrain_last_failure_ms > 100) { - failsafe.terrain_last_failure_ms = 0; - failsafe.terrain_first_failure_ms = 0; - } - } -} - -// terrain failsafe action -void Sub::failsafe_terrain_on_event() -{ - failsafe.terrain = true; - Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_FAILSAFE_OCCURRED); - - // If rangefinder is enabled, we can recover from this failsafe - if (!rangefinder_state.enabled || !auto_terrain_recover_start()) { - failsafe_terrain_act(); - } - - -} - -// Recovery failed, take action -void Sub::failsafe_terrain_act() -{ - switch (g.failsafe_terrain) { - case FS_TERRAIN_HOLD: - if (!set_mode(POSHOLD, MODE_REASON_TERRAIN_FAILSAFE)) { - set_mode(ALT_HOLD, MODE_REASON_TERRAIN_FAILSAFE); - } - AP_Notify::events.failsafe_mode_change = 1; - break; - - case FS_TERRAIN_SURFACE: - set_mode(SURFACE, MODE_REASON_TERRAIN_FAILSAFE); - AP_Notify::events.failsafe_mode_change = 1; - break; - - case FS_TERRAIN_DISARM: - default: - init_disarm_motors(); - } -} - -bool Sub::should_disarm_on_failsafe() -{ - switch (control_mode) { - case STABILIZE: - case ACRO: - // if throttle is zero OR vehicle is landed disarm motors - return ap.throttle_zero; - break; - case AUTO: - // if mission has not started AND vehicle is landed, disarm motors - return !ap.auto_armed; - break; - default: - // used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, PosHold - // if landed disarm - // return ap.land_complete; - return false; - break; - } -} diff --git a/ArduSub/failsafe.cpp b/ArduSub/failsafe.cpp index 3b14e980e0..8edd3f6521 100644 --- a/ArduSub/failsafe.cpp +++ b/ArduSub/failsafe.cpp @@ -69,3 +69,299 @@ void Sub::failsafe_check() } } } + +void Sub::failsafe_battery_event(void) +{ + // // return immediately if low battery event has already been triggered + // if (failsafe.battery) { + // return; + // } + // + // // failsafe check + // if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors.armed()) { + // if (should_disarm_on_failsafe()) { + // init_disarm_motors(); + // } else { + // if (g.failsafe_battery_enabled == FS_BATT_RTL || control_mode == AUTO) { + // set_mode_RTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE); + // } else { + // set_mode_land_with_pause(MODE_REASON_BATTERY_FAILSAFE); + // } + // } + // } + // + // // set the low battery flag + // set_failsafe_battery(true); + // + // // warn the ground station and log to dataflash + // gcs_send_text(MAV_SEVERITY_WARNING,"Low battery"); + // Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED); + +} + +void Sub::failsafe_manual_control_check() +{ +#if CONFIG_HAL_BOARD != HAL_BOARD_SITL + uint32_t tnow = AP_HAL::millis(); + + // Require at least 0.5 Hz update + if (tnow > failsafe.last_manual_control_ms + 2000) { + if (!failsafe.manual_control) { + failsafe.manual_control = true; + set_neutral_controls(); + init_disarm_motors(); + Log_Write_Error(ERROR_SUBSYSTEM_INPUT, ERROR_CODE_FAILSAFE_OCCURRED); + gcs_send_text(MAV_SEVERITY_CRITICAL, "Lost manual control"); + } + return; + } + + failsafe.manual_control = false; +#endif +} + +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) < 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 30 seconds + if (failsafe.internal_pressure && tnow > last_pressure_warn_ms + 30000) { + last_pressure_warn_ms = tnow; + gcs_send_text(MAV_SEVERITY_WARNING, "Internal pressure critical!"); + } +} + +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) < 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 30 seconds + if (failsafe.internal_temperature && tnow > last_temperature_warn_ms + 30000) { + 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; + + // Do nothing if we are dry, or if leak failsafe action is disabled + if (status == false || g.failsafe_leak == FS_LEAK_DISABLED) { + if (failsafe.leak) { + Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_LEAK, ERROR_CODE_FAILSAFE_RESOLVED); + } + failsafe.leak = false; + return; + } + + uint32_t tnow = AP_HAL::millis(); + + // We have a leak + // Always send a warning every 20 seconds + if (tnow > failsafe.last_leak_warn_ms + 20000) { + failsafe.last_leak_warn_ms = tnow; + gcs_send_text(MAV_SEVERITY_CRITICAL, "Leak Detected"); + } + + // Do nothing if we have already triggered the failsafe action, or if the motors are disarmed + if (failsafe.leak) { + return; + } + + failsafe.leak = true; + + Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_LEAK, ERROR_CODE_FAILSAFE_OCCURRED); + + // Handle failsafe action + if (failsafe.leak && g.failsafe_leak == FS_LEAK_SURFACE && motors.armed()) { + set_mode(SURFACE, MODE_REASON_LEAK_FAILSAFE); + } +} + +// failsafe_gcs_check - check for ground station failsafe +void Sub::failsafe_gcs_check() +{ + // return immediately if we have never had contact with a gcs, or if gcs failsafe action is disabled + // this also checks to see if we have a GCS failsafe active, if we do, then must continue to process the logic for recovery from this state. + if (failsafe.last_heartbeat_ms == 0 || (!g.failsafe_gcs && g.failsafe_gcs == FS_GCS_DISABLED)) { + return; + } + + uint32_t tnow = AP_HAL::millis(); + + // Check if we have gotten a GCS heartbeat recently (GCS sysid must match SYSID_MYGCS parameter) + if (tnow < failsafe.last_heartbeat_ms + FS_GCS_TIMEOUT_MS) { + // Log event if we are recovering from previous gcs failsafe + if (failsafe.gcs) { + Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED); + } + failsafe.gcs = false; + return; + } + + ////////////////////////////// + // GCS heartbeat has timed out + ////////////////////////////// + + // Send a warning every 30 seconds + if (tnow > failsafe.last_gcs_warn_ms + 30000) { + failsafe.last_gcs_warn_ms = tnow; + gcs_send_text_fmt(MAV_SEVERITY_WARNING, "MYGCS: %d, heartbeat lost", g.sysid_my_gcs); + } + + // do nothing if we have already triggered the failsafe action, or if the motors are disarmed + if (failsafe.gcs || !motors.armed()) { + return; + } + + // update state, log to dataflash + failsafe.gcs = true; + Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_OCCURRED); + + // handle failsafe action + if (g.failsafe_gcs == FS_GCS_DISARM) { + init_disarm_motors(); + } else if (g.failsafe_gcs == FS_GCS_HOLD && motors.armed()) { + set_mode(ALT_HOLD, MODE_REASON_GCS_FAILSAFE); + } else if (g.failsafe_gcs == FS_GCS_SURFACE && motors.armed()) { + set_mode(SURFACE, MODE_REASON_GCS_FAILSAFE); + } +} + +// executes terrain failsafe if data is missing for longer than a few seconds +// missing_data should be set to true if the vehicle failed to navigate because of missing data, false if navigation is proceeding successfully +void Sub::failsafe_terrain_check() +{ + // trigger with 5 seconds of failures while in AUTO mode + bool valid_mode = (control_mode == AUTO || control_mode == GUIDED); + bool timeout = (failsafe.terrain_last_failure_ms - failsafe.terrain_first_failure_ms) > FS_TERRAIN_TIMEOUT_MS; + bool trigger_event = valid_mode && timeout; + + // check for clearing of event + if (trigger_event != failsafe.terrain) { + if (trigger_event) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"Failsafe terrain triggered"); + failsafe_terrain_on_event(); + } else { + Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_ERROR_RESOLVED); + failsafe.terrain = false; + } + } +} + +// This gets called if mission items are in ALT_ABOVE_TERRAIN frame +// Terrain failure occurs when terrain data is not found, or rangefinder is not enabled or healthy +// set terrain data status (found or not found) +void Sub::failsafe_terrain_set_status(bool data_ok) +{ + uint32_t now = millis(); + + // record time of first and latest failures (i.e. duration of failures) + if (!data_ok) { + failsafe.terrain_last_failure_ms = now; + if (failsafe.terrain_first_failure_ms == 0) { + failsafe.terrain_first_failure_ms = now; + } + } else { + // failures cleared after 0.1 seconds of persistent successes + if (now - failsafe.terrain_last_failure_ms > 100) { + failsafe.terrain_last_failure_ms = 0; + failsafe.terrain_first_failure_ms = 0; + } + } +} + +// terrain failsafe action +void Sub::failsafe_terrain_on_event() +{ + failsafe.terrain = true; + Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_FAILSAFE_OCCURRED); + + // If rangefinder is enabled, we can recover from this failsafe + if (!rangefinder_state.enabled || !auto_terrain_recover_start()) { + failsafe_terrain_act(); + } + + +} + +// Recovery failed, take action +void Sub::failsafe_terrain_act() +{ + switch (g.failsafe_terrain) { + case FS_TERRAIN_HOLD: + if (!set_mode(POSHOLD, MODE_REASON_TERRAIN_FAILSAFE)) { + set_mode(ALT_HOLD, MODE_REASON_TERRAIN_FAILSAFE); + } + AP_Notify::events.failsafe_mode_change = 1; + break; + + case FS_TERRAIN_SURFACE: + set_mode(SURFACE, MODE_REASON_TERRAIN_FAILSAFE); + AP_Notify::events.failsafe_mode_change = 1; + break; + + case FS_TERRAIN_DISARM: + default: + init_disarm_motors(); + } +} + +bool Sub::should_disarm_on_failsafe() +{ + switch (control_mode) { + case STABILIZE: + case ACRO: + // if throttle is zero OR vehicle is landed disarm motors + return ap.throttle_zero; + break; + case AUTO: + // if mission has not started AND vehicle is landed, disarm motors + return !ap.auto_armed; + break; + default: + // used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, PosHold + // if landed disarm + // return ap.land_complete; + return false; + break; + } +} +