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

Copter: Update of land detector

This commit is contained in:
Leonard Hall 2015-04-15 00:34:41 +09:30 committed by Randy Mackay
parent 4d9da2720b
commit ee91be66cf
1 changed files with 31 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
ArduCopter/land_detector.pde
View File

@ -13,13 +13,23 @@ static bool land_complete_maybe()
// called at 50hz
static void update_land_detector()
{
bool climb_rate_low = (abs(climb_rate) < LAND_DETECTOR_CLIMBRATE_MAX);
bool target_climb_rate_low = !pos_control.is_active_z() || (pos_control.get_desired_velocity().z <= LAND_DETECTOR_DESIRED_CLIMBRATE_MAX);
bool motor_at_lower_limit = motors.limit.throttle_lower;
bool throttle_low = (FRAME_CONFIG == HELI_FRAME) || (motors.get_throttle_out() < get_non_takeoff_throttle());
bool not_rotating_fast = (ahrs.get_gyro().length() < LAND_DETECTOR_ROTATION_MAX);
// land detector can not use the following sensors because they are unreliable during landing
// barometer altitude : ground effect can cause errors larger than 4m
// EKF vertical velocity or altitude : poor barometer and large acceleration from ground impact
// earth frame angle or angle error : landing on an uneven surface will force the airframe to match the ground angle
// gyro output : on uneven surface the airframe may rock back an forth after landing
// range finder : tend to be problematic at very short distances
// input throttle : in slow land the input throttle may be only slightly less than hover
if (climb_rate_low && target_climb_rate_low && motor_at_lower_limit && throttle_low && not_rotating_fast) {
// 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);
Vector3f accel_ef = ahrs.get_accel_ef_blended();
accel_ef.z += GRAVITY_MSS;
// check that the airframe is not accelerating (not falling or breaking after fast forward flight)
bool accel_stationary = (accel_ef.length() < 1.0f);
if ( motor_at_lower_limit && accel_stationary) {
if (!ap.land_complete) {
// increase counter until we hit the trigger then set land complete flag
if( land_detector < LAND_DETECTOR_TRIGGER) {
@ -47,8 +57,8 @@ static void update_land_detector()
// has no effect when throttle is above hover throttle
static void update_throttle_low_comp()
{
// manual throttle
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);
} else {
@ -56,8 +66,21 @@ static void update_throttle_low_comp()
}
} else {
// autopilot controlled throttle
// check for aggressive flight requests
const Vector3f angle_target = attitude_control.angle_ef_targets();
if (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
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 )
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);
} else {