From 5b5385715ed9fc8fd70ea8e72f6266ac5f42b55b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 5 Aug 2016 13:36:39 +0900 Subject: [PATCH] Copter: manual modes set_land_complete to false --- ArduCopter/control_acro.cpp | 3 +++ ArduCopter/control_drift.cpp | 5 +++++ ArduCopter/control_stabilize.cpp | 3 +++ ArduCopter/heli_control_acro.cpp | 5 +++++ ArduCopter/heli_control_stabilize.cpp | 5 +++++ 5 files changed, 21 insertions(+) diff --git a/ArduCopter/control_acro.cpp b/ArduCopter/control_acro.cpp index 88445b1966..87130f2a68 100644 --- a/ArduCopter/control_acro.cpp +++ b/ArduCopter/control_acro.cpp @@ -33,6 +33,9 @@ void Copter::acro_run() return; } + // clear landing flag + set_land_complete(false); + motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // convert the input to the desired body frame rate diff --git a/ArduCopter/control_drift.cpp b/ArduCopter/control_drift.cpp index f44a8a8227..e70fefc533 100644 --- a/ArduCopter/control_drift.cpp +++ b/ArduCopter/control_drift.cpp @@ -55,6 +55,11 @@ void Copter::drift_run() return; } + // clear landing flag above zero throttle + if (!ap.throttle_zero) { + set_land_complete(false); + } + // convert pilot input to lean angles get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); diff --git a/ArduCopter/control_stabilize.cpp b/ArduCopter/control_stabilize.cpp index 0a64e02199..7033cad67f 100644 --- a/ArduCopter/control_stabilize.cpp +++ b/ArduCopter/control_stabilize.cpp @@ -34,6 +34,9 @@ void Copter::stabilize_run() return; } + // clear landing flag + set_land_complete(false); + motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // apply SIMPLE mode transform to pilot inputs diff --git a/ArduCopter/heli_control_acro.cpp b/ArduCopter/heli_control_acro.cpp index da5ced0b89..6eaa672b3c 100644 --- a/ArduCopter/heli_control_acro.cpp +++ b/ArduCopter/heli_control_acro.cpp @@ -47,6 +47,11 @@ void Copter::heli_acro_run() } } + // clear landing flag above zero throttle + if (motors.armed() && motors.get_interlock() && motors.rotor_runup_complete() && !ap.throttle_zero) { + set_land_complete(false); + } + if (!motors.has_flybar()){ // convert the input to the desired body frame rate get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw); diff --git a/ArduCopter/heli_control_stabilize.cpp b/ArduCopter/heli_control_stabilize.cpp index 361edf5b41..3c17f77b77 100644 --- a/ArduCopter/heli_control_stabilize.cpp +++ b/ArduCopter/heli_control_stabilize.cpp @@ -46,6 +46,11 @@ void Copter::heli_stabilize_run() } } + // clear landing flag above zero throttle + if (motors.armed() && motors.get_interlock() && motors.rotor_runup_complete() && !ap.throttle_zero) { + set_land_complete(false); + } + // apply SIMPLE mode transform to pilot inputs update_simple_mode();