From ffb605d06d3e306a291dd87bd6afda4ed409f763 Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Thu, 18 Jul 2013 16:02:54 -0400 Subject: [PATCH] TradHeli: Modify the constrain on the auto throttle controller to prevent it from commanding full down collective if we think we are on the ground still. --- ArduCopter/Attitude.pde | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index c37f41e4b2..b1520d3999 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -894,9 +894,17 @@ get_throttle_accel(int16_t z_target_accel) d = g.pid_throttle_accel.get_d(z_accel_error, .01f); - // - // limit the rate +#if FRAME_CONFIG == HELI_FRAME + if (ap.takeoff_complete == true){ + output = constrain_float(p+i+d+g.throttle_cruise, g.throttle_min, g.throttle_max); + } else { + // Avoid harshing the mechanics pushing into the ground + // stab_col_min should gently push down on the ground + output = constrain_float(p+i+d+g.throttle_cruise, motors.stab_col_min*10, motors.stab_col_max*10); + } +#else output = constrain_float(p+i+d+g.throttle_cruise, g.throttle_min, g.throttle_max); +#endif // HELI_FRAME #if LOGGING_ENABLED == ENABLED // log output if PID loggins is on and we are tuning the yaw