Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/ArduCopter.pde
View File

@ -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();

26
ArduCopter/land_detector.pde
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
// 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();
}
}
}