3 #if FRAME_CONFIG == HELI_FRAME 9 bool Copter::ModeStabilize_Heli::init(
bool ignore_checks)
13 pos_control->set_alt_target(0);
16 copter.input_manager.set_use_stab_col(
true);
23 void Copter::ModeStabilize_Heli::run()
25 float target_roll, target_pitch;
26 float target_yaw_rate;
27 float pilot_throttle_scaled;
36 copter.heli_flags.init_targets_on_arming =
true;
37 attitude_control->set_yaw_target_to_current_heading();
38 attitude_control->reset_rate_controller_I_terms();
42 attitude_control->set_yaw_target_to_current_heading();
43 attitude_control->reset_rate_controller_I_terms();
45 copter.heli_flags.init_targets_on_arming=
false;
51 set_land_complete(
false);
58 get_pilot_desired_lean_angles(target_roll, target_pitch,
copter.aparm.angle_max,
copter.aparm.angle_max);
61 target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
64 pilot_throttle_scaled =
copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
67 attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
70 attitude_control->set_throttle_out(pilot_throttle_scaled,
false, g.throttle_filt);
AP_MotorsMatrix motors(400)
bool get_interlock() const
AP_HAL_MAIN_CALLBACKS & copter