From 2425c65e7492b4d1f4cb536d41a05ff35a6cc508 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 27 May 2018 18:12:00 +1000 Subject: [PATCH] Copter: implement 'air-mode' for copter this implements what betaflight calls 'air-mode'. This gives attitude control when throttle is at zero, allowing for zero-throttle maneuvers, plus keeping the copter level on the ground. This was already implemented if an interlock switch was setup, but it should also work with an arming switch. If using an arming switch then throttle should not be considered zero as long as the arming switch hasn't gone low. --- ArduCopter/Copter.h | 1 + ArduCopter/motors.cpp | 3 +++ ArduCopter/radio.cpp | 4 +++- ArduCopter/switches.cpp | 2 ++ 4 files changed, 9 insertions(+), 1 deletion(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index a77841d3f3..58aa63c571 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -333,6 +333,7 @@ private: uint8_t initialised_params : 1; // 25 // true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done uint8_t compass_init_location : 1; // 26 // true when the compass's initial location has been set uint8_t rc_override_enable : 1; // 27 // aux switch rc_override is allowed + uint8_t armed_with_switch : 1; // 28 // we armed using a arming switch }; uint32_t value; } ap_t; diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 789f7c473e..66cb07c073 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -230,6 +230,9 @@ bool Copter::init_arm_motors(bool arming_from_gcs) // Start the arming delay ap.in_arming_delay = true; + // assumed armed without a arming, switch. Overridden in switches.cpp + ap.armed_with_switch = false; + // return success return true; } diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index e320cb8caa..f1e226f12b 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -178,7 +178,9 @@ void Copter::set_throttle_zero_flag(int16_t throttle_control) // if not using throttle interlock and non-zero throttle and not E-stopped, // or using motor interlock and it's enabled, then motors are running, // and we are flying. Immediately set as non-zero - if ((!ap.using_interlock && (throttle_control > 0) && !ap.motor_emergency_stop) || (ap.using_interlock && motors->get_interlock())) { + if ((!ap.using_interlock && (throttle_control > 0) && !ap.motor_emergency_stop) || + (ap.using_interlock && motors->get_interlock()) || + ap.armed_with_switch) { last_nonzero_throttle_ms = tnow_ms; ap.throttle_zero = false; } else if (tnow_ms - last_nonzero_throttle_ms > THROTTLE_ZERO_DEBOUNCE_TIME_MS) { diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index c33cfe294f..57ae8f3d61 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -638,6 +638,8 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) switch (ch_flag) { case AUX_SWITCH_HIGH: init_arm_motors(false); + // remember that we are using an arming switch, for use by set_throttle_zero_flag + ap.armed_with_switch = true; break; case AUX_SWITCH_LOW: init_disarm_motors();