From 8ef88fb98e0505b18a39937e9e1be95c0bcf001e Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Fri, 29 Oct 2021 12:17:45 -0400 Subject: [PATCH] Copter: add collective stick low flag to catch bad setups for land detector --- ArduCopter/Copter.h | 2 ++ ArduCopter/heli.cpp | 16 ++++++++++++++++ ArduCopter/land_detector.cpp | 9 +++++---- 3 files changed, 23 insertions(+), 4 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 5a6bdc6476..c215779810 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -553,6 +553,7 @@ private: uint8_t dynamic_flight : 1; // 0 // true if we are moving at a significant speed (used to turn on/off leaky I terms) uint8_t inverted_flight : 1; // 1 // true for inverted flight mode uint8_t in_autorotation : 1; // 2 // true when heli is in autorotation + bool coll_stk_low ; // 3 // true when collective stick is on lower limit } heli_flags_t; heli_flags_t heli_flags; @@ -761,6 +762,7 @@ private: #if MODE_AUTOROTATE_ENABLED == ENABLED void heli_set_autorotation(bool autotrotation); #endif + void update_collective_low_flag(int16_t throttle_control); // inertia.cpp void read_inertia(); diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index 9daec2cbe4..aad328dd99 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -136,6 +136,7 @@ bool Copter::should_use_landing_swash() const void Copter::heli_update_landing_swash() { motors->set_collective_for_landing(should_use_landing_swash()); + update_collective_low_flag(channel_throttle->get_control_in()); } // convert motor interlock switch's position to desired rotor speed expressed as a value from 0 to 1 @@ -237,4 +238,19 @@ void Copter::heli_set_autorotation(bool autorotation) motors->set_in_autorotation(autorotation); } #endif + +// update collective low flag. Use a debounce time of 400 milliseconds. +void Copter::update_collective_low_flag(int16_t throttle_control) +{ + static uint32_t last_nonzero_collective_ms = 0; + uint32_t tnow_ms = millis(); + + if (throttle_control > 0) { + last_nonzero_collective_ms = tnow_ms; + heli_flags.coll_stk_low = false; + } else if (tnow_ms - last_nonzero_collective_ms > 400) { + heli_flags.coll_stk_low = true; + } +} + #endif // FRAME_CONFIG == HELI_FRAME diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index 48ba18e151..99fa4c0217 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -61,11 +61,12 @@ void Copter::update_land_detector() } else { #if FRAME_CONFIG == HELI_FRAME - // check for both manual collective modes and modes that use altitude hold. For manual collective (called throttle - // because multi's use throttle), check that collective pitch is below mid collective (zero thrust) position. For modes + // check for both manual collective modes and modes that use altitude hold. For manual collective (called throttle + // because multi's use throttle), check that collective pitch is below land min collective position or throttle stick is zero. + // Including the throttle zero check will ensure the conditions where stabilize stick zero position was not below collective min. For modes // that use altitude hold, check that the pilot is commanding a descent and collective is at min allowed for altitude hold modes. - bool motor_at_lower_limit = ((flightmode->has_manual_throttle() && motors->get_below_land_min_coll() && fabsf(ahrs.get_roll()) < M_PI/2.0f) - || (motors->limit.throttle_lower && pos_control->get_vel_desired_cms().z < 0.0f)); + bool motor_at_lower_limit = ((flightmode->has_manual_throttle() && (motors->get_below_land_min_coll() || heli_flags.coll_stk_low) && + fabsf(ahrs.get_roll()) < M_PI/2.0f) || (motors->limit.throttle_lower && pos_control->get_vel_desired_cms().z < 0.0f)); #else // check that the average throttle output is near minimum (less than 12.5% hover throttle) bool motor_at_lower_limit = motors->limit.throttle_lower && attitude_control->is_throttle_mix_min();