diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index d8dd47c116..887f7e2b0c 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1798,8 +1798,9 @@ bool set_throttle_mode( uint8_t new_throttle_mode ) case THROTTLE_AUTO: controller_desired_alt = current_loc.alt; // reset controller desired altitude to current altitude set_new_altitude(current_loc.alt); // by default hold the current altitude - if ( throttle_mode < THROTTLE_HOLD ) { // reset the alt hold I terms if previous throttle mode was manual + if ( throttle_mode <= THROTTLE_MANUAL_TILT_COMPENSATED ) { // reset the alt hold I terms if previous throttle mode was manual reset_throttle_I(); + set_accel_throttle_I_from_pilot_throttle(); } throttle_initialised = true; break; diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 51508426a1..755f11dc88 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -1069,7 +1069,6 @@ get_throttle_land() set_land_complete(true); if( g.rc_3.control_in == 0 || ap.failsafe ) { init_disarm_motors(); - reset_throttle_I(); } } }else{ @@ -1166,6 +1165,12 @@ static void reset_throttle_I(void) g.pid_throttle_accel.reset_I(); } +static void set_accel_throttle_I_from_pilot_throttle(void) +{ + // shift difference between pilot's throttle and hover throttle into accelerometer I + g.pid_throttle_accel.set_integrator(g.rc_3.control_in-g.throttle_cruise); +} + static void reset_stability_I(void) { // Used to balance a quad