From dcc96a57054fe18ff5222e3b971367b905b78ba5 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Tue, 7 Feb 2017 19:42:28 -0500 Subject: [PATCH] Sub: First attempts at AUTO mode --- ArduSub/Parameters.cpp | 14 +++ ArduSub/Parameters.h | 9 +- ArduSub/Sub.h | 10 +- ArduSub/commands_logic.cpp | 10 +- ArduSub/config.h | 2 +- ArduSub/control_auto.cpp | 218 +++++++++++++++++++++++++++++++++++-- ArduSub/defines.h | 38 ++++--- ArduSub/events.cpp | 46 ++++---- 8 files changed, 300 insertions(+), 47 deletions(-) diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index b9b082670c..f6d57e6407 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -230,6 +230,20 @@ const AP_Param::Info Sub::var_info[] = { // @User: Standard GSCALAR(failsafe_temperature_max, "FS_TEMP_MAX", FS_TEMP_MAX_DEFAULT), + // @Param: FS_TERRAIN_ENAB + // @DisplayName: Terrain Failsafe Enable + // @Description: Controls what action to take if terrain information is lost during AUTO mode + // @Values: 0:Disarm, 1:Hold Position, 2:Surface + // @User: Standard + GSCALAR(failsafe_terrain, "FS_TERRAIN_ENAB", FS_TERRAIN_DISARM), + + // @Param: XTRACK_ANG_LIM + // @DisplayName: Crosstrack correction angle limit + // @Description: Maximum allowed angle (in degrees) between current track and desired heading during waypoint navigation + // @Range: 10 90 + // @User: Standard + GSCALAR(xtrack_angle_limit,"XTRACK_ANG_LIM", 45), + // @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 diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index b63ceb36db..ed42643f13 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -395,7 +395,11 @@ public: 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 + k_param_failsafe_temperature_max, // maximum internal temperature in degrees C before failsafe is triggered + + k_param_failsafe_terrain, // terrain failsafe behavior + + k_param_xtrack_angle_limit // angle limit for xtrack correction in degrees }; AP_Int16 format_version; @@ -430,6 +434,9 @@ public: AP_Int8 failsafe_temperature; AP_Int32 failsafe_pressure_max; AP_Int8 failsafe_temperature_max; + AP_Int8 failsafe_terrain; + + AP_Int8 xtrack_angle_limit; AP_Int16 gps_hdop_good; // GPS Hdop value at or below this value represent a good position diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 631da1e7ea..ec4e090b90 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -447,6 +447,8 @@ private: // bearing from current location to the yaw_look_at_WP float yaw_look_at_WP_bearing; + float yaw_xtrack_correct_heading; + // yaw used for YAW_LOOK_AT_HEADING yaw_mode int32_t yaw_look_at_heading; @@ -564,6 +566,7 @@ private: uint32_t last_pilot_heading; uint32_t last_pilot_yaw_input_ms; + uint32_t fs_terrain_recover_start_ms = 0; #if GNDEFFECT_COMPENSATION == ENABLED // ground effect detector @@ -854,7 +857,6 @@ private: 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(); void failsafe_disable(); @@ -1049,6 +1051,12 @@ private: void failsafe_internal_pressure_check(); void failsafe_internal_temperature_check(); + void failsafe_terrain_act(void); + bool auto_terrain_recover_start(void); + void auto_terrain_recover_run(void); + + void translate_wpnav_rp(float &lateral_out, float &forward_out); + bool surface_init(bool ignore_flags); void surface_run(); diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 4bdaadb776..2750efdb15 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -247,7 +247,10 @@ void Sub::exit_mission() // play a tone AP_Notify::events.mission_complete = 1; - init_disarm_motors(); + // Try to enter loiter, if that fails, go to depth hold + if(!auto_loiter_start()) { + set_mode(ALT_HOLD, MODE_REASON_MISSION_END); + } } /********************************************************************************/ @@ -359,6 +362,11 @@ void Sub::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) target_loc.lng = temp_loc.lng; } + // In mavproxy misseditor: Abs = 0 = ALT_FRAME_ABSOLUTE + // Rel = 1 = ALT_FRAME_ABOVE_HOME + // AGL = 3 = ALT_FRAME_ABOVE_TERRAIN + // 2 = ALT_FRAME_ABOVE_ORIGIN, not an option in mavproxy misseditor + // use current altitude if not provided // To-Do: use z-axis stopping point instead of current alt if (target_loc.alt == 0) { diff --git a/ArduSub/config.h b/ArduSub/config.h index 6cc7a4be17..2091b8db4c 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -172,7 +172,7 @@ // 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) + #define FS_TERRAIN_TIMEOUT_MS 1000 // 1 second of unhealthy rangefinder and/or missing terrain data will trigger failsafe #endif #ifndef PREARM_DISPLAY_PERIOD diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index b725e604fe..8afa91262b 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -25,12 +25,6 @@ bool Sub::auto_init(bool ignore_checks) if ((position_ok() && mission.num_commands() > 1) || ignore_checks) { auto_mode = Auto_Loiter; - // reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce change of flips) - if (motors.armed() && ap.land_complete && !mission.starts_with_takeoff_cmd()) { - gcs_send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd"); - return false; - } - // stop ROI from carrying over from previous runs of the mission // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check if (auto_yaw_mode == AUTO_YAW_ROI) { @@ -93,6 +87,10 @@ void Sub::auto_run() case Auto_Loiter: auto_loiter_run(); break; + + case Auto_TerrainRecover: + auto_terrain_recover_run(); + break; } } @@ -216,7 +214,7 @@ void Sub::auto_wp_start(const Location_Class& dest_loc) void Sub::auto_wp_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { + if (!motors.armed() || !motors.get_interlock()) { // To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off // (of course it would be better if people just used take-off) // call attitude controller @@ -243,18 +241,38 @@ void Sub::auto_wp_run() motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller + // TODO logic for terrain tracking target going below fence limit + // TODO implement waypoint radius individually for each waypoint based on cmd.p2 + // TODO fix auto yaw heading to switch to something appropriate when mission complete and switches to loiter failsafe_terrain_set_status(wp_nav.update_wpnav()); + /////////////////////// + // update xy outputs // + + float lateral_out, forward_out; + translate_wpnav_rp(lateral_out, forward_out); + + // Send to forward/lateral outputs + motors.set_lateral(lateral_out); + motors.set_forward(forward_out); + // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); + //////////////////////////// + // update attitude output // + + // get pilot desired lean angles + float target_roll, target_pitch; + get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); + // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true, get_smoothing_gain()); } } @@ -568,7 +586,7 @@ bool Sub::auto_loiter_start() void Sub::auto_loiter_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) { + if (!motors.armed() || !motors.get_interlock()) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); // multicopters do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); @@ -588,8 +606,24 @@ void Sub::auto_loiter_run() // run waypoint and z-axis position controller failsafe_terrain_set_status(wp_nav.update_wpnav()); + /////////////////////// + // update xy outputs // + float lateral_out, forward_out; + translate_wpnav_rp(lateral_out, forward_out); + + // Send to forward/lateral outputs + motors.set_lateral(lateral_out); + motors.set_forward(forward_out); + + // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); + + // get pilot desired lean angles + float target_roll, target_pitch; + get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); + + // roll & pitch from waypoint controller, yaw rate from pilot + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); } // get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter @@ -614,6 +648,10 @@ uint8_t Sub::get_default_auto_yaw_mode(bool rtl) return AUTO_YAW_LOOK_AHEAD; break; + case WP_YAW_BEHAVIOR_CORRECT_XTRACK: + return AUTO_YAW_CORRECT_XTRACK; + break; + case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP: default: return AUTO_YAW_LOOK_AT_NEXT_WP; @@ -677,6 +715,8 @@ void Sub::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int } // get turn speed + // TODO actually implement this, right now, yaw_look_at_heading_slew is unused + // see AP_Float _slew_yaw in AC_AttitudeControl if (is_zero(turn_rate_dps)) { // default to regular auto slew rate yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE; @@ -754,6 +794,20 @@ float Sub::get_auto_heading(void) return initial_armed_bearing; break; + case AUTO_YAW_CORRECT_XTRACK: + { + // Bearing of current track (centidegrees) + float track_bearing = wp_nav.get_bearing_cd(wp_nav.get_wp_origin(), wp_nav.get_wp_destination()); + + // Bearing from current position towards intermediate position target (centidegrees) + float desired_angle = wp_nav.get_loiter_bearing_to_target(); + + float angle_error = wrap_180_cd(desired_angle - track_bearing); + float angle_limited = constrain_float(angle_error, -g.xtrack_angle_limit * 100.0f, g.xtrack_angle_limit * 100.0f); + return wrap_360_cd(track_bearing + angle_limited); + } + break; + case AUTO_YAW_LOOK_AT_NEXT_WP: default: // point towards next waypoint. @@ -763,3 +817,145 @@ float Sub::get_auto_heading(void) } } +// Return true if it is possible to recover from a rangefinder failure +bool Sub::auto_terrain_recover_start() { + // Check rangefinder status to see if recovery is possible + switch(rangefinder.status()) { + + case RangeFinder::RangeFinder_OutOfRangeLow: + case RangeFinder::RangeFinder_OutOfRangeHigh: + + // RangeFinder_Good if just one valid sample was obtained recently, but ::rangefinder_state.alt_healthy + // requires several consecutive valid readings for wpnav to accept rangefinder data + case RangeFinder::RangeFinder_Good: + auto_mode = Auto_TerrainRecover; + break; + + // Not connected or no data + default: + return false; // Rangefinder is not connected, or has stopped responding + } + + // Initialize recovery timeout time + fs_terrain_recover_start_ms = AP_HAL::millis(); + + // Stop mission + mission.stop(); + + // Reset xy target + wp_nav.init_loiter_target(); + + // Reset z axis controller + pos_control.relax_alt_hold_controllers(0.0); + + // initialize vertical speeds and leash lengths + pos_control.set_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up()); + pos_control.set_accel_z(wp_nav.get_accel_z()); + + // Reset vertical position and velocity targets + pos_control.set_alt_target(inertial_nav.get_altitude()); + pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); + + gcs_send_text(MAV_SEVERITY_WARNING, "Attempting auto failsafe recovery"); + return true; +} + +// Attempt recovery from terrain failsafe +// If recovery is successful resume mission +// If recovery fails revert to failsafe action +void Sub::auto_terrain_recover_run() { + float target_climb_rate = 0; + static uint32_t rangefinder_recovery_ms = 0; + + // if not armed set throttle to zero and exit immediately + if (!motors.armed() || !motors.get_interlock()) { + motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); + return; + } + + switch(rangefinder.status()) { + + case RangeFinder::RangeFinder_OutOfRangeLow: + target_climb_rate = wp_nav.get_speed_up(); + rangefinder_recovery_ms = 0; + break; + + case RangeFinder::RangeFinder_OutOfRangeHigh: + target_climb_rate = wp_nav.get_speed_down(); + rangefinder_recovery_ms = 0; + break; + + case RangeFinder::RangeFinder_Good: // exit on success (recovered rangefinder data) + + target_climb_rate = 0; // Attempt to hold current depth + + if(rangefinder_state.alt_healthy) { + + // Start timer as soon as rangefinder is healthy + if(rangefinder_recovery_ms == 0) { + rangefinder_recovery_ms = AP_HAL::millis(); + pos_control.relax_alt_hold_controllers(0.0); // Reset alt hold targets + } + + // 1.5 seconds of healthy rangefinder means we can resume mission with terrain enabled + if(AP_HAL::millis() > rangefinder_recovery_ms + 1500) { + gcs_send_text(MAV_SEVERITY_INFO, "Terrain failsafe recovery successful!"); + failsafe_terrain_set_status(true); // Reset failsafe timers + failsafe.terrain = false; // Clear flag + auto_mode = Auto_Loiter; // Switch back to loiter for next iteration + mission.resume(); // Resume mission + rangefinder_recovery_ms = 0; // Reset for subsequent recoveries + } + + } + break; + + // Not connected, or no data + default: + // Terrain failsafe recovery has failed, terrain data is not available + // and rangefinder is not connected, or has stopped responding + gcs_send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery failure: No Rangefinder!"); + failsafe_terrain_act(); + rangefinder_recovery_ms = 0; + return; + } + + // exit on failure (timeout) + if(AP_HAL::millis() > fs_terrain_recover_start_ms + FS_TERRAIN_RECOVER_TIMEOUT_MS) { + // Recovery has failed, revert to failsafe action + gcs_send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery timeout!"); + failsafe_terrain_act(); + } + + // run loiter controller + wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); + + /////////////////////// + // update xy targets // + float lateral_out, forward_out; + translate_wpnav_rp(lateral_out, forward_out); + + // Send to forward/lateral outputs + motors.set_lateral(lateral_out); + motors.set_forward(forward_out); + + ///////////////////// + // update z target // + pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, true); + pos_control.update_z_controller(); + + //////////////////////////// + // update angular targets // + float target_roll = 0; + float target_pitch = 0; + + // convert pilot input to lean angles + // To-Do: convert get_pilot_desired_lean_angles to return angles as floats + get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); + + float target_yaw_rate = 0; + + // call attitude controller + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); +} diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 83d57abbce..f8c371ba50 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -17,12 +17,13 @@ // Autopilot Yaw Mode enumeration enum autopilot_yaw_mode { - AUTO_YAW_HOLD = 0, // pilot controls the heading - AUTO_YAW_LOOK_AT_NEXT_WP = 1, // point towards next waypoint (no pilot input accepted) - AUTO_YAW_ROI = 2, // point towards a location held in roi_WP (no pilot input accepted) - AUTO_YAW_LOOK_AT_HEADING = 3, // point towards a particular angle (not pilot input accepted) - AUTO_YAW_LOOK_AHEAD = 4, // point in the direction the copter is moving - AUTO_YAW_RESETTOARMEDYAW = 5, // point towards heading at time motors were armed + AUTO_YAW_HOLD = 0, // pilot controls the heading + AUTO_YAW_LOOK_AT_NEXT_WP = 1, // point towards next waypoint (no pilot input accepted) + AUTO_YAW_ROI = 2, // point towards a location held in roi_WP (no pilot input accepted) + AUTO_YAW_LOOK_AT_HEADING = 3, // point towards a particular angle (not pilot input accepted) + AUTO_YAW_LOOK_AHEAD = 4, // point in the direction the copter is moving + AUTO_YAW_RESETTOARMEDYAW = 5, // point towards heading at time motors were armed + AUTO_YAW_CORRECT_XTRACK = 6 // steer the sub in order to correct for crosstrack error during line following }; // Ch6... Ch12 aux switch control @@ -197,6 +198,7 @@ enum tuning_func { #define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP 1 // auto pilot will face next waypoint or home during rtl #define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL 2 // auto pilot will face next waypoint except when doing RTL at which time it will stay in it's last #define WP_YAW_BEHAVIOR_LOOK_AHEAD 3 // auto pilot will look ahead during missions and rtl (primarily meant for traditional helicotpers) +#define WP_YAW_BEHAVIOR_CORRECT_XTRACK 4 // point towards intermediate position target during line following // Auto modes enum AutoMode { @@ -208,7 +210,8 @@ enum AutoMode { Auto_Circle, Auto_Spline, Auto_NavGuided, - Auto_Loiter + Auto_Loiter, + Auto_TerrainRecover }; // Guided modes @@ -467,16 +470,25 @@ enum ThrowModeState { #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 +#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 +#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 +#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 +#define FS_TEMP_DISABLED 0 +#define FS_TEMP_WARN_ONLY 1 + +// Terrain failsafe actions for AUTO mode +#define FS_TERRAIN_DISARM 0 +#define FS_TERRAIN_HOLD 1 +#define FS_TERRAIN_SURFACE 2 + +// Amount of time to attempt recovery of valid rangefinder data before +// initiating terrain failsafe action +#define FS_TERRAIN_RECOVER_TIMEOUT_MS 10000 // EKF failsafe definitions (FS_EKF_ENABLE parameter) #define FS_EKF_ACTION_LAND 1 // switch to LAND mode on EKF failsafe diff --git a/ArduSub/events.cpp b/ArduSub/events.cpp index f186f2d507..6056fa3a19 100644 --- a/ArduSub/events.cpp +++ b/ArduSub/events.cpp @@ -222,6 +222,7 @@ void Sub::failsafe_terrain_check() // 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); @@ -230,6 +231,8 @@ void Sub::failsafe_terrain_check() } } +// 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) { @@ -254,30 +257,35 @@ void Sub::failsafe_terrain_set_status(bool data_ok) void Sub::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); + // If rangefinder is enabled, we can recover from this failsafe + if(!rangefinder_state.enabled || !auto_terrain_recover_start()) { + failsafe_terrain_act(); } + + } -// 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 Sub::set_mode_RTL_or_land_with_pause(mode_reason_t reason) -{ -// // attempt to switch to RTL, if this fails then switch to Land -// if (!set_mode(RTL, reason)) { -// // set mode to land will trigger mode change notification to pilot -// set_mode_land_with_pause(reason); -// } else { -// // alert pilot to mode change -// AP_Notify::events.failsafe_mode_change = 1; -// } +// 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() {