diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 44059ddb6a..f25e5fa972 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -243,8 +243,10 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current } // set_accel_throttle_I_from_pilot_throttle - smoothes transition from pilot controlled throttle to autopilot throttle -void Copter::set_accel_throttle_I_from_pilot_throttle(float pilot_throttle) +void Copter::set_accel_throttle_I_from_pilot_throttle() { + // get last throttle input sent to attitude controller + float pilot_throttle = constrain_float(attitude_control.get_throttle_in(), 0.0f, 1.0f); // shift difference between pilot's throttle and hover throttle into accelerometer I g.pid_accel_z.set_integrator((pilot_throttle-motors.get_throttle_hover()) * 1000.0f); } diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 4367e1379f..10e03b9bd5 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -691,7 +691,7 @@ private: float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt); void auto_takeoff_set_start_alt(void); void auto_takeoff_attitude_run(float target_yaw_rate); - void set_accel_throttle_I_from_pilot_throttle(float pilot_throttle); + void set_accel_throttle_I_from_pilot_throttle(); void update_poscon_alt_max(); void rotate_body_frame_to_NE(float &x, float &y); void gcs_send_heartbeat(void); diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index ef69f45beb..330ca5793f 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -274,7 +274,7 @@ void Copter::exit_mode(control_mode_t old_control_mode, control_mode_t new_contr // smooth throttle transition when switching from manual to automatic flight modes if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed() && !ap.land_complete) { // this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle - set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->get_control_in())); + set_accel_throttle_I_from_pilot_throttle(); } // cancel any takeoffs in progress