Browse Source

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.

master
Robert Lefebvre 12 years ago committed by Randy Mackay
parent
commit
ffb605d06d
  1. 12
      ArduCopter/Attitude.pde

12
ArduCopter/Attitude.pde

@ -894,9 +894,17 @@ get_throttle_accel(int16_t z_target_accel) @@ -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

Loading…
Cancel
Save