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
e39b38ba96
This commit is contained in:
Matthias Grob 2022-10-25 00:09:40 +02:00 committed by Silvan Fuhrer
parent ac646d32e6
commit 7667883385
2 changed files with 1 additions and 4 deletions

View File

@ -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

View File

@ -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;