3 #if MODE_ACRO_ENABLED == ENABLED 5 #if FRAME_CONFIG == HELI_FRAME 11 bool Copter::ModeAcro_Heli::init(
bool ignore_checks)
14 attitude_control->use_flybar_passthrough(
motors->has_flybar(),
motors->supports_yaw_passthrough());
16 motors->set_acro_tail(
true);
19 copter.input_manager.set_use_stab_col(
false);
27 void Copter::ModeAcro_Heli::run()
29 float target_roll, target_pitch, target_yaw;
30 float pilot_throttle_scaled;
39 copter.heli_flags.init_targets_on_arming=
true;
40 attitude_control->set_attitude_target_to_current_attitude();
41 attitude_control->reset_rate_controller_I_terms();
45 attitude_control->set_attitude_target_to_current_attitude();
46 attitude_control->reset_rate_controller_I_terms();
48 copter.heli_flags.init_targets_on_arming=
false;
54 set_land_complete(
false);
57 if (!
motors->has_flybar()){
59 get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw);
61 if (
motors->supports_yaw_passthrough()) {
65 target_yaw = channel_yaw->get_control_in_zero_dz();
69 attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
75 float roll_in = channel_roll->get_control_in_zero_dz();
76 float pitch_in = channel_pitch->get_control_in_zero_dz();
79 if (
motors->supports_yaw_passthrough()) {
83 yaw_in = channel_yaw->get_control_in_zero_dz();
88 yaw_in = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
92 attitude_control->passthrough_bf_roll_pitch_rate_yaw(roll_in, pitch_in, yaw_in);
96 pilot_throttle_scaled =
copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
99 attitude_control->set_throttle_out(pilot_throttle_scaled,
false, g.throttle_filt);
103 #endif //MODE_ACRO_ENABLED == ENABLED
AP_MotorsMatrix motors(400)
bool get_interlock() const
AP_HAL_MAIN_CALLBACKS & copter