diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 826a6d883d..bc5ddfb918 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -450,6 +450,9 @@ void Copter::three_hz_loop() // check if we've lost contact with the ground station failsafe_gcs_check(); + // check if we've lost terrain data + failsafe_terrain_check(); + #if AC_FENCE == ENABLED // check if we have breached a fence fence_check(); diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 8b4a797532..a5b6a23f4d 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -282,10 +282,13 @@ private: 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 int8_t radio_counter; // number of iterations with throttle below throttle_fs_value uint32_t last_heartbeat_ms; // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe + uint32_t terrain_first_failure_ms; // the first time terrain data access failed - used to calculate the duration of the failure + uint32_t terrain_last_failure_ms; // the most recent time terrain data access failed } failsafe; // sensor health for logging @@ -853,6 +856,9 @@ private: void failsafe_battery_event(void); void failsafe_gcs_check(); void failsafe_gcs_off_event(void); + void failsafe_terrain_check(); + void failsafe_terrain_set_status(bool data_ok); + void failsafe_terrain_on_event(); void set_mode_RTL_or_land_with_pause(mode_reason_t reason); void update_events(); void failsafe_enable(); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 605d6c0872..900383ed85 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -33,7 +33,7 @@ NOINLINE void Copter::send_heartbeat(mavlink_channel_t chan) uint32_t custom_mode = control_mode; // set system as critical if any failsafe have triggered - if (failsafe.radio || failsafe.battery || failsafe.gcs || failsafe.ekf) { + if (failsafe.radio || failsafe.battery || failsafe.gcs || failsafe.ekf || failsafe.terrain) { system_status = MAV_STATE_CRITICAL; } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index dcc1765f45..4c2753ec2d 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -205,6 +205,11 @@ #define FS_RADIO_TIMEOUT_MS 500 // RC Radio Failsafe triggers after 500 miliseconds with No RC Input #endif +// missing terrain data failsafe +#ifndef FS_TERRAIN_TIMEOUT_MS + #define FS_TERRAIN_TIMEOUT_MS 5000 // 5 seconds of missing terrain data will trigger failsafe (RTL) +#endif + #ifndef PREARM_DISPLAY_PERIOD # define PREARM_DISPLAY_PERIOD 30 #endif diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 51a9637070..6a35524766 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -117,7 +117,8 @@ enum mode_reason_t { MODE_REASON_GPS_GLITCH, MODE_REASON_MISSION_END, MODE_REASON_THROTTLE_LAND_ESCAPE, - MODE_REASON_FENCE_BREACH + MODE_REASON_FENCE_BREACH, + MODE_REASON_TERRAIN_FAILSAFE }; // Tuning enumeration @@ -382,6 +383,7 @@ enum ThrowModeState { #define ERROR_SUBSYSTEM_FAILSAFE_ADSB 20 #define ERROR_SUBSYSTEM_TERRAIN 21 #define ERROR_SUBSYSTEM_NAVIGATION 22 +#define ERROR_SUBSYSTEM_FAILSAFE_TERRAIN 23 // general error codes #define ERROR_CODE_ERROR_RESOLVED 0 #define ERROR_CODE_FAILED_TO_INITIALISE 1 diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 9c16070692..c5861f902e 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -130,6 +130,62 @@ void Copter::failsafe_gcs_off_event(void) Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED); } +// 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 Copter::failsafe_terrain_check() +{ + // trigger with 5 seconds of failures while in AUTO mode + bool valid_mode = (control_mode == AUTO || control_mode == GUIDED || control_mode == RTL); + 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) { + failsafe_terrain_on_event(); + } else { + Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_ERROR_RESOLVED); + failsafe.terrain = false; + } + } +} + +// set terrain data status (found or not found) +void Copter::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 Copter::failsafe_terrain_on_event() +{ + failsafe.terrain = true; + gcs_send_text(MAV_SEVERITY_CRITICAL,"Failsafe: Terrain data missing"); + Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_FAILSAFE_OCCURRED); + + if (should_disarm_on_failsafe()) { + init_disarm_motors(); + } else if (control_mode == RTL) { + rtl_restart_without_terrain(); + } else { + set_mode_RTL_or_land_with_pause(MODE_REASON_TERRAIN_FAILSAFE); + } +} + // set_mode_RTL_or_land_with_pause - sets mode to RTL if possible or LAND with 4 second delay before descent starts // this is always called from a failsafe so we trigger notification to pilot void Copter::set_mode_RTL_or_land_with_pause(mode_reason_t reason)