From 6848dc17e552a3867dbfd48431396808da5e8c6c Mon Sep 17 00:00:00 2001 From: Jason Short Date: Wed, 8 Feb 2012 21:26:48 -0800 Subject: [PATCH] Added safety to prevent shutdown of motors during Auto-land --- ArduCopter/commands_logic.pde | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 231f8585c3..8af5db3609 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -381,12 +381,14 @@ static bool verify_land_sonar() // if we are low or don't seem to be decending much, increment ground detector if(current_loc.alt < 40 || abs(climb_rate) < 20) { landing_boost++; // reduce the throttle at twice the normal rate - if(ground_detector++ > 30) { + if(ground_detector++ >= 30) { land_complete = true; - ground_detector = 0; + ground_detector = 30; icount = 1; - // init disarm motors - init_disarm_motors(); + if(g.rc_3.control_in == 0){ + // init disarm motors + init_disarm_motors(); + } return true; } } @@ -412,11 +414,13 @@ static bool verify_land_baro() if(current_loc.alt < 150 ){ if(abs(climb_rate) < 20) { landing_boost++; - if(ground_detector++ > 30) { + if(ground_detector++ >= 30) { land_complete = true; - ground_detector = 0; - // init disarm motors - init_disarm_motors(); + ground_detector = 30; + if(g.rc_3.control_in == 0){ + // init disarm motors + init_disarm_motors(); + } return true; } }