diff --git a/ArduCopter/land_detector.pde b/ArduCopter/land_detector.pde index 20544864a6..8a70ca9b67 100644 --- a/ArduCopter/land_detector.pde +++ b/ArduCopter/land_detector.pde @@ -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 {