From 9b84bd3f6ed89f5545b55403eafdb4076a8ec150 Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Sat, 15 Apr 2017 20:34:13 -0400 Subject: [PATCH 1/2] AC_AttitudeControl: reset target attitude added method to reset current vehicle attitude to support swash behaviors in Trad Heli --- libraries/AC_AttitudeControl/AC_AttitudeControl.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 4454bc3abdcfa..eac3d2e4ae137 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -107,6 +107,9 @@ class AC_AttitudeControl { // reset rate controller I terms void reset_rate_controller_I_terms(); + // Sets attitude target to vehicle attitude + void set_attitude_target_to_current_attitude() { _attitude_target_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned()); } + // Sets yaw target to vehicle heading void set_yaw_target_to_current_heading() { shift_ef_yaw_target(degrees(_ahrs.yaw - _attitude_target_euler_angle.z)*100.0f); } From 1e8132539b33d2c229d8fa83b5b39daede925241 Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Thu, 26 Oct 2017 18:53:49 -0400 Subject: [PATCH 2/2] Copter: heli: Update swashplate behavior change swashplate behavior on ground in Acro, Stabilize, and AltHold flight modes. See discussion here: https://discuss.ardupilot.org/t/tradheli-swashplate-behavior-while-on-the-ground-and-potential-fix-to-issue-5396/22463/18 --- ArduCopter/control_althold.cpp | 20 ++++++++++++++++++-- ArduCopter/heli_control_acro.cpp | 8 +++++--- ArduCopter/heli_control_stabilize.cpp | 4 +++- 3 files changed, 26 insertions(+), 6 deletions(-) diff --git a/ArduCopter/control_althold.cpp b/ArduCopter/control_althold.cpp index 0bd3d94533c60..3ddd12c013742 100644 --- a/ArduCopter/control_althold.cpp +++ b/ArduCopter/control_althold.cpp @@ -74,18 +74,24 @@ void Copter::althold_run() motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); + attitude_control->reset_rate_controller_I_terms(); + attitude_control->set_yaw_target_to_current_heading(); #if FRAME_CONFIG == HELI_FRAME // force descent rate and call position controller pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); + heli_flags.init_targets_on_arming=true; #else - attitude_control->reset_rate_controller_I_terms(); - attitude_control->set_yaw_target_to_current_heading(); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero #endif pos_control->update_z_controller(); break; case AltHold_Takeoff: +#if FRAME_CONFIG == HELI_FRAME + if (heli_flags.init_targets_on_arming) { + heli_flags.init_targets_on_arming=false; + } +#endif // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); @@ -121,8 +127,18 @@ void Copter::althold_run() motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); } +#if FRAME_CONFIG == HELI_FRAME + if (heli_flags.init_targets_on_arming) { + attitude_control->reset_rate_controller_I_terms(); + attitude_control->set_yaw_target_to_current_heading(); + if (motors->get_interlock()) { + heli_flags.init_targets_on_arming=false; + } + } +#else attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); +#endif attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero pos_control->update_z_controller(); diff --git a/ArduCopter/heli_control_acro.cpp b/ArduCopter/heli_control_acro.cpp index c1037a77b9d31..386559d8500cf 100644 --- a/ArduCopter/heli_control_acro.cpp +++ b/ArduCopter/heli_control_acro.cpp @@ -35,12 +35,14 @@ void Copter::heli_acro_run() if(!motors->armed()) { heli_flags.init_targets_on_arming=true; - attitude_control->set_yaw_target_to_current_heading(); + attitude_control->set_attitude_target_to_current_attitude(); + attitude_control->reset_rate_controller_I_terms(); } if(motors->armed() && heli_flags.init_targets_on_arming) { - attitude_control->set_yaw_target_to_current_heading(); - if (motors->rotor_speed_above_critical()) { + attitude_control->set_attitude_target_to_current_attitude(); + attitude_control->reset_rate_controller_I_terms(); + if (motors->get_interlock()) { heli_flags.init_targets_on_arming=false; } } diff --git a/ArduCopter/heli_control_stabilize.cpp b/ArduCopter/heli_control_stabilize.cpp index f225c1c8e9b60..09a9668154258 100644 --- a/ArduCopter/heli_control_stabilize.cpp +++ b/ArduCopter/heli_control_stabilize.cpp @@ -35,11 +35,13 @@ void Copter::heli_stabilize_run() if(!motors->armed()) { heli_flags.init_targets_on_arming=true; attitude_control->set_yaw_target_to_current_heading(); + attitude_control->reset_rate_controller_I_terms(); } if(motors->armed() && heli_flags.init_targets_on_arming) { attitude_control->set_yaw_target_to_current_heading(); - if (motors->rotor_speed_above_critical()) { + attitude_control->reset_rate_controller_I_terms(); + if (motors->get_interlock()) { heli_flags.init_targets_on_arming=false; } }