25 float takeoff_climb_rate = 0.0f;
35 float target_roll, target_pitch;
50 }
else if (!
ap.auto_armed ||
ap.land_complete) {
57 switch (althold_state) {
62 attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
65 #if FRAME_CONFIG == HELI_FRAME 68 heli_flags.init_targets_on_arming=
true;
76 #if FRAME_CONFIG == HELI_FRAME 77 if (heli_flags.init_targets_on_arming) {
78 heli_flags.init_targets_on_arming=
false;
100 attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
110 if (target_climb_rate < 0.0
f) {
116 #if FRAME_CONFIG == HELI_FRAME 117 if (heli_flags.init_targets_on_arming) {
120 if (
motors->get_interlock()) {
121 heli_flags.init_targets_on_arming=
false;
128 attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
136 #if AC_AVOID_ENABLED == ENABLED 138 copter.avoid.adjust_roll_pitch(target_roll, target_pitch,
copter.aparm.angle_max);
142 attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
145 if (
copter.rangefinder_alt_ok()) {
virtual float get_velocity_z() const=0
float get_alt_target() const
bool takeoff_triggered(float target_climb_rate) const
float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt)
virtual void set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend)
void takeoff_get_climb_rates(float &pilot_climb_rate, float &takeoff_climb_rate)
float get_avoidance_adjusted_climbrate(float target_rate)
void update_z_controller()
uint16_t get_pilot_speed_dn(void)
void update_simple_mode(void)
AC_AttitudeControl_t *& attitude_control
float get_pilot_desired_climb_rate(float throttle_control)
float get_pilot_desired_yaw_rate(int16_t stick_angle)
AP_Float pilot_takeoff_alt
void add_takeoff_climb_rate(float climb_rate_cms, float dt)
void takeoff_timer_start(float alt_cm)
int16_t get_control_in() const
void relax_alt_hold_controllers(float throttle_setting)
void set_desired_velocity_z(float vel_z_cms)
void set_speed_z(float speed_down, float speed_up)
void set_throttle_takeoff(void)
void set_accel_z(float accel_cmss)
void get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const
DESIRED_THROTTLE_UNLIMITED
virtual void set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend)
AC_PosControl *& pos_control
takeoff_state_t & takeoff_state
float constrain_float(const float amt, const float low, const float high)
bool init(bool ignore_checks) override
AP_HAL_MAIN_CALLBACKS & copter
AP_InertialNav & inertial_nav
RC_Channel *& channel_yaw
void set_land_complete(bool b)
RC_Channel *& channel_throttle
void set_alt_target_to_current_alt()