From 3600f941e5e709b76033f6c2aa42640484176bbd Mon Sep 17 00:00:00 2001 From: lthall Date: Tue, 23 Apr 2024 03:37:26 +0930 Subject: [PATCH 1/4] Copter: Land Detector: Include angle checks --- ArduCopter/land_detector.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index d7b82c497d203..95743cfafa384 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -100,6 +100,14 @@ void Copter::update_land_detector() } #endif + // check for aggressive flight requests - requested roll or pitch angle below 15 degrees + const Vector3f angle_target = attitude_control->get_att_target_euler_cd(); + bool large_angle_request = angle_target.xy().length() > LAND_CHECK_LARGE_ANGLE_CD; + + // check for large external disturbance - angle error over 30 degrees + const float angle_error = attitude_control->get_att_error_angle_deg(); + bool large_angle_error = (angle_error > LAND_CHECK_ANGLE_ERROR_DEG); + // check that the airframe is not accelerating (not falling or braking after fast forward flight) bool accel_stationary = (land_accel_ef_filter.get().length() <= LAND_DETECTOR_ACCEL_MAX * land_detector_scalar); @@ -116,7 +124,7 @@ void Copter::update_land_detector() const bool WoW_check = true; #endif - if (motor_at_lower_limit && throttle_mix_at_min && accel_stationary && descent_rate_low && rangefinder_check && WoW_check) { + if (motor_at_lower_limit && throttle_mix_at_min && !large_angle_request && !large_angle_error && accel_stationary && descent_rate_low && rangefinder_check && WoW_check) { // landed criteria met - increment the counter and check if we've triggered if( land_detector_count < land_trigger_sec*scheduler.get_loop_rate_hz()) { land_detector_count++; From 187d5e3baca5ede6b1cfcf3884b1b92a14b603f3 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 24 Apr 2024 18:24:08 +0930 Subject: [PATCH 2/4] Copter: Land Detector: add hash define for LAND_DETECTOR_VEL_Z_MAX --- ArduCopter/config.h | 3 +++ ArduCopter/land_detector.cpp | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index bd139ff23a58e..6a1ef097ed311 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -382,6 +382,9 @@ #ifndef LAND_DETECTOR_ACCEL_MAX # define LAND_DETECTOR_ACCEL_MAX 1.0f // vehicle acceleration must be under 1m/s/s #endif +#ifndef LAND_DETECTOR_VEL_Z_MAX +# define LAND_DETECTOR_VEL_Z_MAX 1.0f // vehicle vertical velocity must be under 1m/s +#endif ////////////////////////////////////////////////////////////////////////////// // Flight mode definitions diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index 95743cfafa384..c075e6c9b8958 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -112,7 +112,7 @@ void Copter::update_land_detector() bool accel_stationary = (land_accel_ef_filter.get().length() <= LAND_DETECTOR_ACCEL_MAX * land_detector_scalar); // check that vertical speed is within 1m/s of zero - bool descent_rate_low = fabsf(inertial_nav.get_velocity_z_up_cms()) < 100 * land_detector_scalar; + bool descent_rate_low = fabsf(inertial_nav.get_velocity_z_up_cms()) < 100.0 * LAND_DETECTOR_VEL_Z_MAX * land_detector_scalar; // if we have a healthy rangefinder only allow landing detection below 2 meters bool rangefinder_check = (!rangefinder_alt_ok() || rangefinder_state.alt_cm_filt.get() < LAND_RANGEFINDER_MIN_ALT_CM); From ea326787e0ec62c3639412168879e94a2ead8898 Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Wed, 24 Apr 2024 23:21:26 -0400 Subject: [PATCH 3/4] Tools: modify heli autotest to prevent autorotation test failure --- Tools/autotest/helicopter.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/Tools/autotest/helicopter.py b/Tools/autotest/helicopter.py index b7339368967b5..90ac04d741889 100644 --- a/Tools/autotest/helicopter.py +++ b/Tools/autotest/helicopter.py @@ -362,8 +362,12 @@ def AutoRotation(self, timeout=600): timeout=timeout) self.context_collect('STATUSTEXT') self.progress("Triggering autorotate by raising interlock") + self.set_rc(3, 1000) self.set_rc(8, 1000) + self.wait_statustext("SS Glide Phase", check_context=True) + + self.change_mode('STABILIZE') self.wait_statustext(r"SIM Hit ground at ([0-9.]+) m/s", check_context=True, regex=True) @@ -962,6 +966,8 @@ def do_RTL(self, **kwargs): def tests(self): '''return list of all tests''' + ret = ([self.AutoRotation, self.ManAutoRotation,]) + return ret ret = vehicle_test_suite.TestSuite.tests(self) ret.extend([ self.AVCMission, From 7f48d18930fe5d7e80439816818b3445cd41487a Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Wed, 24 Apr 2024 23:25:11 -0400 Subject: [PATCH 4/4] Tools: clean up heli autotest file --- Tools/autotest/helicopter.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/Tools/autotest/helicopter.py b/Tools/autotest/helicopter.py index 90ac04d741889..0f02f32242fbe 100644 --- a/Tools/autotest/helicopter.py +++ b/Tools/autotest/helicopter.py @@ -966,8 +966,6 @@ def do_RTL(self, **kwargs): def tests(self): '''return list of all tests''' - ret = ([self.AutoRotation, self.ManAutoRotation,]) - return ret ret = vehicle_test_suite.TestSuite.tests(self) ret.extend([ self.AVCMission,