diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 1a630bb382..ed2b523444 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -919,7 +919,7 @@ static void throttle_loop() read_inertial_altitude(); // update throttle_low_comp value (controls priority of throttle vs attitude control) - update_throttle_low_comp(); + update_throttle_thr_mix(); // check auto_armed status update_auto_armed(); diff --git a/ArduCopter/land_detector.pde b/ArduCopter/land_detector.pde index 10960e1501..76571f92c5 100644 --- a/ArduCopter/land_detector.pde +++ b/ArduCopter/land_detector.pde @@ -22,7 +22,7 @@ static void update_land_detector() // input throttle : in slow land the input throttle may be only slightly less than hover // check that the average throttle output is near minimum (less than 12.5% hover throttle) - bool motor_at_lower_limit = motors.limit.throttle_lower && (motors.get_throttle_low_comp() < 0.125f); + bool motor_at_lower_limit = motors.limit.throttle_lower && motors.is_throttle_mix_min(); Vector3f accel_ef = ahrs.get_accel_ef_blended(); accel_ef.z += GRAVITY_MSS; @@ -55,39 +55,41 @@ static void update_land_detector() set_land_complete_maybe(land_detector >= LAND_DETECTOR_MAYBE_TRIGGER_SEC*MAIN_LOOP_RATE); } -// update_throttle_low_comp - sets motors throttle_low_comp value depending upon vehicle state +// update_throttle_thr_mix - sets motors throttle_low_comp value depending upon vehicle state // low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle // has no effect when throttle is above hover throttle -static void update_throttle_low_comp() +static void update_throttle_thr_mix() { if (mode_has_manual_throttle(control_mode)) { // manual throttle if(!motors.armed() || g.rc_3.control_in <= 0) { - motors.set_throttle_low_comp(0.1f); + motors.set_throttle_mix_min(); } else { - motors.set_throttle_low_comp(0.5f); + motors.set_throttle_mix_mid(); } } else { // autopilot controlled throttle - // check for aggressive flight requests + // check for aggressive flight requests - requested roll or pitch angle below 15 degrees const Vector3f angle_target = attitude_control.angle_ef_targets(); bool large_angle_request = (pythagorous2(angle_target.x, angle_target.y) > 1500.0f); - // check for large external disturbance + // check for large external disturbance - angle error over 30 degrees const Vector3f angle_error = attitude_control.angle_bf_error(); bool large_angle_error = (pythagorous2(angle_error.x, angle_error.y) > 3000.0f); - // check for large acceleration ( falling ) + // check for large acceleration - falling or high turbulence Vector3f accel_ef = ahrs.get_accel_ef_blended(); accel_ef.z += GRAVITY_MSS; bool accel_moving = (accel_ef.length() > 3.0f); - if ( large_angle_request || large_angle_error || accel_moving) { - // if target lean angle is over 15 degrees set high - motors.set_throttle_low_comp(0.9f); + // check for requested decent + bool descent_not_demanded = pos_control.get_desired_velocity().z >= 0.0f; + + if ( large_angle_request || large_angle_error || accel_moving || descent_not_demanded) { + motors.set_throttle_mix_max(); } else { - motors.set_throttle_low_comp(0.1f); + motors.set_throttle_mix_min(); } } }