diff --git a/ArduCopterMega/flight_control.pde b/ArduCopterMega/flight_control.pde index 24c04648ee..037fdc7367 100644 --- a/ArduCopterMega/flight_control.pde +++ b/ArduCopterMega/flight_control.pde @@ -25,10 +25,14 @@ void calc_nav_throttle() long error = constrain(altitude_error, -300, 300); if(altitude_sensor == BARO) { + if(error > 0){ + pid_baro_throttle.kP(.25); + }else{ + pid_baro_throttle.kP(.05); + } nav_throttle = pid_baro_throttle.get_pid(error, delta_ms_fast_loop, 1.0); - // limit output of throttle control - nav_throttle = throttle_cruise + constrain(nav_throttle, -50, 100); + nav_throttle = throttle_cruise + constrain(nav_throttle, -15, 70); } else { // SONAR