Copter: Add additional check to Throttle Mix/Comp

This commit is contained in:
Leonard Hall 2015-05-03 23:51:44 +09:30 committed by Randy Mackay
parent 0f894ac1a8
commit 7fd632200c
2 changed files with 15 additions and 13 deletions

View File

@ -919,7 +919,7 @@ static void throttle_loop()
read_inertial_altitude(); read_inertial_altitude();
// update throttle_low_comp value (controls priority of throttle vs attitude control) // update throttle_low_comp value (controls priority of throttle vs attitude control)
update_throttle_low_comp(); update_throttle_thr_mix();
// check auto_armed status // check auto_armed status
update_auto_armed(); update_auto_armed();

View File

@ -22,7 +22,7 @@ static void update_land_detector()
// input throttle : in slow land the input throttle may be only slightly less than hover // 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) // 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(); Vector3f accel_ef = ahrs.get_accel_ef_blended();
accel_ef.z += GRAVITY_MSS; 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); 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 // 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 // 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)) { if (mode_has_manual_throttle(control_mode)) {
// manual throttle // manual throttle
if(!motors.armed() || g.rc_3.control_in <= 0) { if(!motors.armed() || g.rc_3.control_in <= 0) {
motors.set_throttle_low_comp(0.1f); motors.set_throttle_mix_min();
} else { } else {
motors.set_throttle_low_comp(0.5f); motors.set_throttle_mix_mid();
} }
} else { } else {
// autopilot controlled throttle // 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(); const Vector3f angle_target = attitude_control.angle_ef_targets();
bool large_angle_request = (pythagorous2(angle_target.x, angle_target.y) > 1500.0f); 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(); const Vector3f angle_error = attitude_control.angle_bf_error();
bool large_angle_error = (pythagorous2(angle_error.x, angle_error.y) > 3000.0f); 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(); Vector3f accel_ef = ahrs.get_accel_ef_blended();
accel_ef.z += GRAVITY_MSS; accel_ef.z += GRAVITY_MSS;
bool accel_moving = (accel_ef.length() > 3.0f); bool accel_moving = (accel_ef.length() > 3.0f);
if ( large_angle_request || large_angle_error || accel_moving) { // check for requested decent
// if target lean angle is over 15 degrees set high bool descent_not_demanded = pos_control.get_desired_velocity().z >= 0.0f;
motors.set_throttle_low_comp(0.9f);
if ( large_angle_request || large_angle_error || accel_moving || descent_not_demanded) {
motors.set_throttle_mix_max();
} else { } else {
motors.set_throttle_low_comp(0.1f); motors.set_throttle_mix_min();
} }
} }
} }