diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 9f414b538e..073d0e697e 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -207,11 +207,6 @@ float Copter::get_non_takeoff_throttle() return MAX(0,motors.get_throttle_hover()/2.0f); } -float Copter::get_takeoff_trigger_throttle() -{ - return channel_throttle->get_control_mid() + g.takeoff_trigger_dz; -} - // get_surface_tracking_climb_rate - hold copter at the desired distance above the ground // returns climb rate (in cm/s) which should be passed to the position controller float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index c7a4f04331..7af0f0f88f 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -650,7 +650,6 @@ private: float get_pilot_desired_throttle(int16_t throttle_control); float get_pilot_desired_climb_rate(float throttle_control); float get_non_takeoff_throttle(); - float get_takeoff_trigger_throttle(); 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); diff --git a/ArduCopter/arming_checks.cpp b/ArduCopter/arming_checks.cpp index f981064719..7b3aec91ce 100644 --- a/ArduCopter/arming_checks.cpp +++ b/ArduCopter/arming_checks.cpp @@ -719,7 +719,7 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) // check throttle is not too high - skips checks if arming from GCS in Guided if (!(arming_from_gcs && (control_mode == GUIDED || control_mode == GUIDED_NOGPS))) { // above top of deadband is too always high - if (channel_throttle->get_control_in() > get_takeoff_trigger_throttle()) { + if (get_pilot_desired_climb_rate(channel_throttle->get_control_in()) > 0.0f) { if (display_failure) { #if FRAME_CONFIG == HELI_FRAME gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high"); diff --git a/ArduCopter/control_poshold.cpp b/ArduCopter/control_poshold.cpp index 8182e79252..12c009b2c7 100644 --- a/ArduCopter/control_poshold.cpp +++ b/ArduCopter/control_poshold.cpp @@ -165,7 +165,7 @@ void Copter::poshold_run() takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); // check for take-off - if (ap.land_complete && (takeoff_state.running || channel_throttle->get_control_in() > get_takeoff_trigger_throttle())) { + if (ap.land_complete && (takeoff_state.running || target_climb_rate > 0.0f)) { if (!takeoff_state.running) { takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); }