fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 13:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

LandDetectorMC: enforce that LNDMC_Z_VEL_MAX is larger than MPC_LAND_CRWL/MPC_LAND_SPEED (#20614)

Browse Source 
* LandDetectorMC: enforce that LNDMC_Z_VEL_MAX * 1.2 is below *MPC_LAND_CRWL/MPC_LAND_SPEED

Otherwise the _in_descend flag doesn't get set correctly during the last part
of the landing, where the descend speed is at MPC_LAND_CRWL or LAND_SPEED.
The _in_descend flag is set it the velocity setpoint is >1.1*LNDMC_Z_VEL_MAX.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: Matthias Grob <maetugr@gmail.com>
This commit is contained in:
Silvan Fuhrer
2022-11-17 10:56:26 +01:00
committed by GitHub
co-authored by Matthias Grob
parent e58ad581a0
commit 798cc4f01c
3 changed files with 24 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/MulticopterLandDetector.cpp
+15
View File
@@ -79,6 +79,8 @@ MulticopterLandDetector::MulticopterLandDetector()
_paramHandle.minThrottle = param_find("MPC_THR_MIN");
_paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE");
_paramHandle.hoverThrottle = param_find("MPC_THR_HOVER");
_paramHandle.landSpeed = param_find("MPC_LAND_SPEED");
_paramHandle.crawlSpeed = param_find("MPC_LAND_CRWL");
_minimum_thrust_8s_hysteresis.set_hysteresis_time_from(false, 8_s);
}
@@ -118,6 +120,19 @@ void MulticopterLandDetector::_update_params()
{
param_get(_paramHandle.minThrottle, &_params.minThrottle);
param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
param_get(_paramHandle.landSpeed, &_params.landSpeed);
param_get(_paramHandle.crawlSpeed, &_params.crawlSpeed);
// 1.2 corresponds to the margin between the default parameters LNDMC_Z_VEL_MAX = MPC_LAND_CRWL / 1.2
const float lndmc_upper_threshold = math::min(_params.crawlSpeed, _params.landSpeed) / 1.2f;
if (_param_lndmc_z_vel_max.get() > lndmc_upper_threshold) {
PX4_ERR("LNDMC_Z_VEL_MAX > MPC_LAND_CRWL or MPC_LAND_SPEED, updating %.3f -> %.3f",
(double)_param_lndmc_z_vel_max.get(), (double)(lndmc_upper_threshold));
_param_lndmc_z_vel_max.set(lndmc_upper_threshold);
_param_lndmc_z_vel_max.commit_no_notification();
}
int32_t use_hover_thrust_estimate = 0;
param_get(_paramHandle.useHoverThrustEstimate, &use_hover_thrust_estimate);