mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-03 04:03:59 -04:00
Sub: First attempts at AUTO mode
This commit is contained in:
Jacob Walser
Andrew Tridgell
parent
96303c3092
commit
dcc96a5705
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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();
|
||||
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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());
|
||||
}
|
||||
|
||||
@ -23,6 +23,7 @@ enum autopilot_yaw_mode {
|
||||
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
|
||||
@ -478,6 +481,15 @@ enum ThrowModeState {
|
||||
#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
|
||||
#define FS_EKF_ACTION_ALTHOLD 2 // switch to ALTHOLD mode on EKF failsafe
|
||||
|
||||
@ -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() {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user