diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 8850e2cd53..d035fbfa69 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -459,6 +459,9 @@ #ifndef LAND_DETECTOR_TRIGGER # define LAND_DETECTOR_TRIGGER 50 // number of 50hz iterations with near zero climb rate and low throttle that triggers landing complete. #endif +#ifndef LAND_DETECTOR_MAYBE_TRIGGER + # define LAND_DETECTOR_MAYBE_TRIGGER 10 // number of 50hz iterations with near zero climb rate and low throttle that means we might be landed (used to reset horizontal position targets to prevent tipping over) +#endif #ifndef LAND_DETECTOR_CLIMBRATE_MAX # define LAND_DETECTOR_CLIMBRATE_MAX 30 // vehicle climb rate must be between -30 and +30 cm/s #endif diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index e4dddc22ea..f34692f20b 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -287,6 +287,11 @@ static void auto_land_run() return; } + // init loiter targets when maybe landed + if(land_maybe_complete()) { + wp_nav.loiter_soften_for_landing(); + } + // process pilot's input if (!failsafe.radio) { if (g.land_repositioning) { diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index 237aad939d..7299e8fe58 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -73,6 +73,11 @@ static void land_gps_run() return; } + // relax loiter target if we might be landed + if(land_maybe_complete()) { + wp_nav.loiter_soften_for_landing(); + } + // process pilot inputs if (!failsafe.radio) { if (g.land_repositioning) { @@ -187,6 +192,11 @@ static float get_throttle_land() } } +static bool land_maybe_complete() +{ + return (ap.land_complete || land_detector > LAND_DETECTOR_MAYBE_TRIGGER); +} + // update_land_detector - checks if we have landed and updates the ap.land_complete flag // called at 50hz static void update_land_detector() diff --git a/ArduCopter/control_loiter.pde b/ArduCopter/control_loiter.pde index 3883342e6f..6240290a7b 100644 --- a/ArduCopter/control_loiter.pde +++ b/ArduCopter/control_loiter.pde @@ -68,6 +68,11 @@ static void loiter_run() wp_nav.clear_pilot_desired_acceleration(); } + // relax loiter target if we might be landed + if (land_maybe_complete()) { + wp_nav.loiter_soften_for_landing(); + } + // when landed reset targets and output zero throttle if (ap.land_complete) { wp_nav.init_loiter_target(); diff --git a/ArduCopter/control_poshold.pde b/ArduCopter/control_poshold.pde index beec114b5b..f9d1c7f4c0 100644 --- a/ArduCopter/control_poshold.pde +++ b/ArduCopter/control_poshold.pde @@ -183,6 +183,11 @@ static void poshold_run() } } + // relax loiter target if we might be landed + if(land_maybe_complete()) { + wp_nav.loiter_soften_for_landing(); + } + // if landed initialise loiter targets, set throttle to zero and exit if (ap.land_complete) { wp_nav.init_loiter_target(); diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde index 2caafe4e92..f0ece2b864 100644 --- a/ArduCopter/control_rtl.pde +++ b/ArduCopter/control_rtl.pde @@ -349,6 +349,11 @@ static void rtl_land_run() return; } + // relax loiter target if we might be landed + if(land_maybe_complete()) { + wp_nav.loiter_soften_for_landing(); + } + // process pilot's input if (!failsafe.radio) { if (g.land_repositioning) {