20 return !must_navigate;
31 #if FRAME_CONFIG == HELI_FRAME 33 if (!
motors->rotor_runup_complete()) {
39 #if MODE_GUIDED_ENABLED == ENABLED 95 takeoff_climb_rate = 0.0f;
100 static const float takeoff_accel = 50.0f;
107 if (time_elapsed <= time_to_max_speed) {
108 height_gained = 0.5f*takeoff_accel*
sq(time_elapsed) + takeoff_minspeed*time_elapsed;
110 height_gained = 0.5f*takeoff_accel*
sq(time_to_max_speed) + takeoff_minspeed*time_to_max_speed +
121 takeoff_climb_rate = 0.0f;
126 takeoff_climb_rate = speed;
129 if (pilot_climb_rate < 0.0
f) {
131 if (takeoff_climb_rate + pilot_climb_rate > 0.0
f) {
132 takeoff_climb_rate = takeoff_climb_rate + pilot_climb_rate;
133 pilot_climb_rate = 0;
136 pilot_climb_rate = pilot_climb_rate + takeoff_climb_rate;
137 takeoff_climb_rate = 0.0f;
141 if (pilot_climb_rate > takeoff_climb_rate) {
142 pilot_climb_rate = pilot_climb_rate - takeoff_climb_rate;
144 pilot_climb_rate = 0.0f;
154 if (!
motors->armed() || !
ap.auto_armed || !
motors->get_interlock() ||
ap.land_complete) {
167 float nav_roll, nav_pitch;
173 #if FRAME_CONFIG == HELI_FRAME 175 hover_roll_trim_scalar_slew = 0;
185 attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate);
float auto_takeoff_no_nav_alt_cm
void set_auto_armed(bool b)
AC_AttitudeControl_t * attitude_control
float get_altitude() const
int32_t get_pitch() const
void takeoff_timer_start(float alt_cm)
void takeoff_get_climb_rates(float &pilot_climb_rate, float &takeoff_climb_rate)
void set_limit_accel_xy(void)
AP_InertialNav_NavEKF inertial_nav
void auto_takeoff_attitude_run(float target_yaw_rate)
takeoff_state_t takeoff_state
float get_speed_up() const
Location_Class current_loc
bool do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
bool current_mode_has_user_takeoff(bool must_navigate)
void auto_takeoff_set_start_alt(void)
bool takeoff_start(float final_alt_above_home)
AC_PosControl * pos_control
control_mode_t control_mode