From 7667883385865680d5c1687ca0e61e708ca1cee5 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 25 Oct 2022 00:09:40 +0200 Subject: [PATCH] MulticopterLandDetector: make in descend detection depend on vertical speed threshold It's very important that the in descend detection is always at a strictly higher velocity than the vertical movement check. This combination is basically used to check for vertical downwards velocity tracking. Desired descend, no movement -> ground If in descend threshold is lower than vertical movement it is by definition even with perfect tracking the case that with any velocity between the two thresholds there is no movement even though a descend is commanded. See first fix of this problem #7831 e39b38ba96971245aaf6d2b1c249868c8717665e --- src/modules/land_detector/MulticopterLandDetector.cpp | 2 +- src/modules/land_detector/MulticopterLandDetector.h | 3 --- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 20c0f38623..6741e45c4f 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -202,7 +202,7 @@ bool MulticopterLandDetector::_get_ground_contact_state() if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) { // Setpoints can be NAN _in_descend = PX4_ISFINITE(trajectory_setpoint.velocity[2]) - && (trajectory_setpoint.velocity[2] > DESCENT_TRAJECTORY_VZ_THRESHOLD); + && (trajectory_setpoint.velocity[2] >= 1.1f * _param_lndmc_z_vel_max.get()); } // ground contact requires commanded descent until landed diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 6493ec4261..1e473eff83 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -87,9 +87,6 @@ private: /** Distance above ground below which entering ground contact state is possible when distance to ground is available. */ static constexpr float DIST_FROM_GROUND_THRESHOLD = 1.0f; - /** Down velocity threshold for setting "in_descend" flag */ - static constexpr float DESCENT_TRAJECTORY_VZ_THRESHOLD = 0.3f; - /** Handles for interesting parameters. **/ struct { param_t minThrottle;