fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 03:00:35 +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-07 17:42:03 +01: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
* 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 {