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

HTE: decrease sensitivity with speed

Browse Source 
VTOL planes are getting lift from the wing when flying in MC mode at
high speed. They (and some other drones) also get extra drag when
climbing and descending at high speed, corrupting the hover thrust
estimate.

To avoid this, two speed thresholds (vertical and horizontal) are defined
above which the sensitivity of the estimator is decreased by linearly
increasing the observation noise.
This commit is contained in:
bresch
2021-09-03 15:20:09 +02:00
committed by Daniel Agar
parent c67d943158
commit 904ed57aba
5 changed files with 44 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
+7 -3
View File
@@ -168,9 +168,13 @@ void MulticopterHoverThrustEstimator::Run()
// Inform the hover thrust estimator about the measured vertical
// acceleration (positive acceleration is up) and the current thrust (positive thrust is up)
// Guard against fast up and down motions biasing the estimator due to large drag and prop wash effects
if (fabsf(local_pos.vz) < 2.f) {
_hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2]);
}
const float meas_noise_coeff_z = fmaxf((fabsf(local_pos.vz) - _param_hte_vz_thr.get()) + 1.f, 1.f);
const float meas_noise_coeff_xy = fmaxf((matrix::Vector2f(local_pos.vx,
local_pos.vy).norm() - _param_hte_vxy_thr.get()) + 1.f,
1.f);
_hover_thrust_ekf.setMeasurementNoiseScale(fmaxf(meas_noise_coeff_xy, meas_noise_coeff_z));
_hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2]);
bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f);