fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 02:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW land detector: tighten thresholds in airspeed-less case

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-12-09 20:55:09 -05:00
committed by Daniel Agar
parent 16621b19b3
commit 24d871f792
2 changed files with 25 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/FixedwingLandDetector.cpp
+12 -3
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. * Copyright (c) 2013-2021 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -80,9 +80,12 @@ bool FixedwingLandDetector::_get_landed_state()
airspeed_validated_s airspeed_validated{}; airspeed_validated_s airspeed_validated{};
_airspeed_validated_sub.copy(&airspeed_validated); _airspeed_validated_sub.copy(&airspeed_validated);
bool airspeed_invalid = false;
// set _airspeed_filtered to 0 if airspeed data is invalid // set _airspeed_filtered to 0 if airspeed data is invalid
if (!PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) || hrt_elapsed_time(&airspeed_validated.timestamp) > 1_s) { if (!PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) || hrt_elapsed_time(&airspeed_validated.timestamp) > 1_s) {
_airspeed_filtered = 0.0f; _airspeed_filtered = 0.0f;
airspeed_invalid = true;
} else { } else {
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * airspeed_validated.true_airspeed_m_s; _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * airspeed_validated.true_airspeed_m_s;
@@ -93,10 +96,16 @@ bool FixedwingLandDetector::_get_landed_state()
const float acc_hor = matrix::Vector2f(_acceleration).norm(); const float acc_hor = matrix::Vector2f(_acceleration).norm();
_xy_accel_filtered = _xy_accel_filtered * 0.8f + acc_hor * 0.18f; _xy_accel_filtered = _xy_accel_filtered * 0.8f + acc_hor * 0.18f;
// make thresholds tighter if airspeed is invalid
const float vel_xy_max_threshold = airspeed_invalid ? 0.7f * _param_lndfw_vel_xy_max.get() :
_param_lndfw_vel_xy_max.get();
const float vel_z_max_threshold = airspeed_invalid ? 0.7f * _param_lndfw_vel_z_max.get() :
_param_lndfw_vel_z_max.get();
// Crude land detector for fixedwing. // Crude land detector for fixedwing.
landDetected = _airspeed_filtered < _param_lndfw_airspd.get() landDetected = _airspeed_filtered < _param_lndfw_airspd.get()
&& _velocity_xy_filtered < _param_lndfw_vel_xy_max.get() && _velocity_xy_filtered < vel_xy_max_threshold
&& _velocity_z_filtered < _param_lndfw_vel_z_max.get() && _velocity_z_filtered < vel_z_max_threshold
&& _xy_accel_filtered < _param_lndfw_xyaccel_max.get(); && _xy_accel_filtered < _param_lndfw_xyaccel_max.get();
} else { } else {
src/modules/land_detector/land_detector_params_fw.c
+13 -9
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2014-2017 PX4 Development Team. All rights reserved. * Copyright (c) 2014-2021 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -32,9 +32,11 @@
****************************************************************************/ ****************************************************************************/
/** /**
* Fixedwing max horizontal velocity * Fixed-wing land detector: Max horizontal velocity threshold
* *
* Maximum horizontal velocity allowed in the landed state * Maximum horizontal velocity allowed in the landed state.
* A factor of 0.7 is applied in case of airspeed-less flying
* (either because no sensor is present or sensor data got invalid in flight).
* *
* @unit m/s * @unit m/s
* @min 0.5 * @min 0.5
@@ -46,9 +48,11 @@
PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f); PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f);
/** /**
* Fixedwing max climb rate * Fixed-wing land detector: Max vertiacal velocity threshold
* *
* Maximum vertical velocity allowed in the landed state * Maximum vertical velocity allowed in the landed state.
* A factor of 0.7 is applied in case of airspeed-less flying
* (either because no sensor is present or sensor data got invalid in flight).
* *
* @unit m/s * @unit m/s
* @min 0.1 * @min 0.1
@@ -57,10 +61,10 @@ PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f);
* *
* @group Land Detector * @group Land Detector
*/ */
PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 3.0f); PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 2.0f);
/** /**
* Fixedwing max horizontal acceleration * Fixed-wing land detector: Max horizontal acceleration
* *
* Maximum horizontal (x,y body axes) acceleration allowed in the landed state * Maximum horizontal (x,y body axes) acceleration allowed in the landed state
* *
@@ -74,12 +78,12 @@ PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 3.0f);
PARAM_DEFINE_FLOAT(LNDFW_XYACC_MAX, 8.0f); PARAM_DEFINE_FLOAT(LNDFW_XYACC_MAX, 8.0f);
/** /**
* Airspeed max * Fixed-wing land detector: Max airspeed
* *
* Maximum airspeed allowed in the landed state * Maximum airspeed allowed in the landed state
* *
* @unit m/s * @unit m/s
* @min 4 * @min 2
* @max 20 * @max 20
* @decimal 1 * @decimal 1
* *