Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 16:46:09 +08:00
Code Issues Actions 10 Packages Projects Releases Wiki Activity

MC position controller: Stop holding position once ground contact is established.

Browse Source 
This ensures the system does not tip over on landings. Tested in SITL with a GPS drift model.
This commit is contained in:
Lorenz Meier
2017-01-14 02:05:59 +01:00
parent 5b54d78128
commit 12767c8538
1 changed files with 11 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+11 -2
View File
@@ -1823,16 +1823,25 @@ MulticopterPositionControl::control_position(float dt)
/* descend stabilized, we're landing */ /* descend stabilized, we're landing */
if (!_in_landing && !_lnd_reached_ground if (!_in_landing && !_lnd_reached_ground
&& (float)fabsf(_acc_z_lp) < 0.1f && (float)fabsf(_acc_z_lp) < 0.1f
&& _vel_z_lp > 0.5f * _params.land_speed) { && _vel_z_lp > 0.6f * _params.land_speed) {
_in_landing = true; _in_landing = true;
} }
float land_z_threshold = 0.1f;
/* assume ground, cut thrust */ /* assume ground, cut thrust */
if (_in_landing if (_in_landing
&& _vel_z_lp < 0.1f) { && _vel_z_lp < land_z_threshold) {
thr_max = 0.0f; thr_max = 0.0f;
_in_landing = false; _in_landing = false;
_lnd_reached_ground = true; _lnd_reached_ground = true;
} else if (_in_landing
&& _vel_z_lp < math::min(0.3f * _params.land_speed, 2.5f * land_z_threshold)) {
/* not on ground but with ground contact, stop position and velocity control */
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
} }
/* once we assumed to have reached the ground always cut the thrust. /* once we assumed to have reached the ground always cut the thrust.