From 534ba89756083cc496dc30004edb9d5bd693362b Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Wed, 8 Jul 2015 15:10:50 -0400 Subject: [PATCH] Copter: Tradheli manage yaw better on the ground. --- ArduCopter/control_althold.cpp | 15 +++++++++++++++ ArduCopter/heli_control_acro.cpp | 7 +++++-- ArduCopter/heli_control_stabilize.cpp | 5 ++++- 3 files changed, 24 insertions(+), 3 deletions(-) diff --git a/ArduCopter/control_althold.cpp b/ArduCopter/control_althold.cpp index d475287b08..6c7a346d54 100644 --- a/ArduCopter/control_althold.cpp +++ b/ArduCopter/control_althold.cpp @@ -44,7 +44,22 @@ void Copter::althold_run() target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); #if FRAME_CONFIG == HELI_FRAME + // helicopters are held on the ground until rotor speed runup has finished bool takeoff_triggered = (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()) && motors.rotor_runup_complete()); + + if(!motors.armed()) { + heli_flags.init_targets_on_arming=true; + attitude_control.set_yaw_target_to_current_heading(); + } + + // if we have recently armed and rotor is just ramping up speed, relax yaw controller + if(motors.armed() && heli_flags.init_targets_on_arming) { + attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); + if (motors.rotor_speed_above_critical()) { + heli_flags.init_targets_on_arming=false; + } + } #else bool takeoff_triggered = (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle())); #endif diff --git a/ArduCopter/heli_control_acro.cpp b/ArduCopter/heli_control_acro.cpp index c00dd053c4..ec65e5dec0 100644 --- a/ArduCopter/heli_control_acro.cpp +++ b/ArduCopter/heli_control_acro.cpp @@ -33,10 +33,13 @@ void Copter::heli_acro_run() heli_flags.init_targets_on_arming=true; attitude_control.set_yaw_target_to_current_heading(); } - + if(motors.armed() && heli_flags.init_targets_on_arming) { - heli_flags.init_targets_on_arming=false; attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); + if (motors.rotor_speed_above_critical()) { + heli_flags.init_targets_on_arming=false; + } } // send RC inputs direct into motors library for use during manual passthrough for helicopter setup diff --git a/ArduCopter/heli_control_stabilize.cpp b/ArduCopter/heli_control_stabilize.cpp index f18ee88d28..b9ef80977f 100644 --- a/ArduCopter/heli_control_stabilize.cpp +++ b/ArduCopter/heli_control_stabilize.cpp @@ -36,8 +36,11 @@ void Copter::heli_stabilize_run() } if(motors.armed() && heli_flags.init_targets_on_arming) { - heli_flags.init_targets_on_arming=false; attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); + if (motors.rotor_speed_above_critical()) { + heli_flags.init_targets_on_arming=false; + } } // send RC inputs direct into motors library for use during manual passthrough for helicopter setup