From cd75e15ddb6d58a8e5530ecb459ca6ff791dac16 Mon Sep 17 00:00:00 2001 From: jaxxzer Date: Sun, 6 Mar 2016 17:39:32 -0500 Subject: [PATCH] Sub: land mode will surface the craft --- ArduSub/control_land.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduSub/control_land.cpp b/ArduSub/control_land.cpp index e114dbee60..22378d835a 100644 --- a/ArduSub/control_land.cpp +++ b/ArduSub/control_land.cpp @@ -157,7 +157,7 @@ void Sub::land_nogps_run() } // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately - if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) { + if(!ap.auto_armed || ap.at_surface || !motors.get_interlock()) { // multicopters do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); @@ -168,8 +168,8 @@ void Sub::land_nogps_run() } #else // disarm when the landing detector says we've landed - if (ap.land_complete) { - init_disarm_motors(); + if (ap.at_surface) { + set_mode(ALT_HOLD); } #endif return;