dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FW land detector: tighten thresholds in airspeed-less case 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2021-12-07 17:42:03 +01:00 committed by Daniel Agar
parent 16621b19b3
commit 24d871f792
2 changed files with 25 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/land_detector/FixedwingLandDetector.cpp
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
* modification, are permitted provided that the following conditions
@ -80,9 +80,12 @@ bool FixedwingLandDetector::_get_landed_state()
airspeed_validated_s airspeed_validated{};
_airspeed_validated_sub.copy(&airspeed_validated);
bool airspeed_invalid = false;
// 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) {
_airspeed_filtered = 0.0f;
airspeed_invalid = true;
} else {
_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();
_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.
landDetected = _airspeed_filtered < _param_lndfw_airspd.get()
&& _velocity_xy_filtered < _param_lndfw_vel_xy_max.get()
&& _velocity_z_filtered < _param_lndfw_vel_z_max.get()
&& _velocity_xy_filtered < vel_xy_max_threshold
&& _velocity_z_filtered < vel_z_max_threshold
&& _xy_accel_filtered < _param_lndfw_xyaccel_max.get();
} else {

22
src/modules/land_detector/land_detector_params_fw.c
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
* 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
* @min 0.5
@ -46,9 +48,11 @@
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
* @min 0.1
@ -57,10 +61,10 @@ PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f);
*
* @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
*
@ -74,12 +78,12 @@ PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 3.0f);
PARAM_DEFINE_FLOAT(LNDFW_XYACC_MAX, 8.0f);
/**
* Airspeed max
* Fixed-wing land detector: Max airspeed
*
* Maximum airspeed allowed in the landed state
*
* @unit m/s
* @min 4
* @min 2
* @max 20
* @decimal 1
*