13 if (
copter.position_ok() || ignore_checks) {
32 gcs().
send_text(MAV_SEVERITY_CRITICAL,
"Restarting RTL - Terrain data missing");
134 if (!
motors->armed() || !
ap.auto_armed || !
motors->get_interlock()) {
141 float target_yaw_rate = 0;
142 if (!
copter.failsafe.radio) {
145 if (!
is_zero(target_yaw_rate)) {
192 if (!
motors->armed() || !
ap.auto_armed || !
motors->get_interlock()) {
199 float target_yaw_rate = 0;
200 if (!
copter.failsafe.radio) {
203 if (!
is_zero(target_yaw_rate)) {
260 float target_roll = 0.0f;
261 float target_pitch = 0.0f;
262 float target_yaw_rate = 0.0f;
265 if (!
motors->armed() || !
ap.auto_armed || !
motors->get_interlock()) {
273 if (!
copter.failsafe.radio) {
291 ap.land_repo_active =
true;
356 if (!
motors->armed() || !
ap.auto_armed ||
ap.land_complete || !
motors->get_interlock()) {
362 if (
ap.land_complete && disarm_on_land) {
363 copter.init_disarm_motors();
409 #if AC_RALLY == ENABLED 416 int32_t curr_alt =
copter.current_loc.alt;
419 rtl_path.terrain_used =
copter.terrain_use() && terrain_following_allowed;
422 int32_t origin_terr_alt, return_target_terr_alt;
443 int32_t target_alt = MAX(
rtl_path.return_target.alt, 0);
450 float rtl_return_dist_cm =
rtl_path.return_target.get_distance(
rtl_path.origin_point) * 100.0f;
459 #if AC_FENCE == ENABLED 467 if (
rtl_path.return_target.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, target_alt)) {
468 float fence_alt =
copter.fence.get_safe_alt_max()*100.0f;
469 if (target_alt > fence_alt) {
471 rtl_path.return_target.alt -= (target_alt - fence_alt);
void set_speed_xy(float speed_cms)
void get_stopping_point_xy(Vector3f &stopping_point) const
void zero_throttle_and_relax_ac()
virtual float get_velocity_z() const=0
bool reached_wp_destination() const
void land_run(bool disarm_on_land)
void set_mode_to_default(bool rtl)
void set_alt_target_with_slew(float alt_cm, float dt)
int32_t get_pitch() const
AP_Int16 throttle_behavior
void get_stopping_point_z(Vector3f &stopping_point) const
void update_z_controller()
const struct Location & get_home(void) const
void Log_Write_Event(uint8_t id)
void update_simple_mode(void)
autopilot_yaw_mode default_mode(bool rtl) const
AC_AttitudeControl_t *& attitude_control
void init_target(const Vector3f &position)
auto wrap_180_cd(const T angle) -> decltype(wrap_180(angle, 100.f))
float get_pilot_desired_yaw_rate(int16_t stick_angle)
struct Copter::ModeRTL::@9 rtl_path
AP_Int8 land_repositioning
void land_run_horizontal_control()
bool set_wp_destination(const Location_Class &destination)
autopilot_yaw_mode mode() const
void land_run_vertical_control(bool pause_descent=false)
int32_t get_wp_bearing_to_destination() const
void build_path(bool terrain_following_allowed)
bool landing_gear_should_be_deployed() const override
#define THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND
void set_mode(autopilot_yaw_mode new_mode)
#define RTL_ABS_MIN_CLIMB
void update(float ekfGndSpdLimit, float ekfNavVelGainScaler)
int16_t get_control_in() const
#define ERROR_CODE_FAILED_TO_SET_DESTINATION
const Vector3f & get_wp_destination() const
bool is_zero(const T fVal1)
void set_desired_velocity_z(float vel_z_cms)
void set_fast_waypoint(bool fast)
void loiterathome_start()
float get_wp_distance_to_destination() const
#define ERROR_CODE_MISSING_TERRAIN_DATA
void wp_and_spline_init()
#define RTL_MIN_CONE_SLOPE
#define DATA_LAND_CANCELLED_BY_PILOT
void send_text(MAV_SEVERITY severity, const char *fmt,...)
void get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const
DESIRED_THROTTLE_UNLIMITED
AC_PosControl *& pos_control
bool init(bool ignore_checks) override
void compute_return_target(bool terrain_following_allowed)
AP_HAL_MAIN_CALLBACKS & copter
#define ERROR_CODE_RESTARTED_RTL
uint32_t _loiter_start_time
#define LAND_CANCEL_TRIGGER_THR
#define ERROR_SUBSYSTEM_TERRAIN
void set_pilot_desired_acceleration(float euler_roll_angle_cd, float euler_pitch_angle_cd, float dt)
int32_t wp_bearing() const override
float & ekfNavVelGainScaler
void set_target_to_stopping_point_z()
AP_InertialNav & inertial_nav
RC_Channel *& channel_yaw
uint32_t wp_distance() const override
int32_t get_pitch() const
#define ERROR_SUBSYSTEM_NAVIGATION
#define AC_FENCE_TYPE_ALT_MAX
void restart_without_terrain()
void set_alt_target_to_current_alt()
float get_angle_max_cd() const