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

mc_pos_control_main:

Browse Source 
for landing, set velocity to zero and consider thrust_sp in body frame
instead of NED frame. Also limit thrust_sp_body_z to be larger than 0.0f
This commit is contained in:
Dennis Mannhart 2017-01-18 11:21:52 +01:00 committed by Lorenz Meier
parent 58983e4c52
commit f297c45f78
1 changed files with 18 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -1750,12 +1750,25 @@ MulticopterPositionControl::control_position(float dt)
thrust_sp(1) = 0.0f;
}
/* if still or already on ground command a zero XY velocity */
/* if still or already on ground command zero xy velcoity and zero xy thrust_sp in body frame to consider uneven ground */
if (_vehicle_land_detected.ground_contact) {
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
_vel_sp(0) = _vel(0);
_vel_sp(1) = _vel(1);
/* thrust setpoint in body frame*/
math::Vector<3> thrust_sp_body = _R.transposed() * thrust_sp;
/* we dont want to make any correction in body x and y*/
thrust_sp_body(0) = 0.0f;
thrust_sp_body(1) = 0.0f;
/* make sure z component of thrust_sp_body is larger than 0 (positive thrust is downward) */
thrust_sp_body(2) = thrust_sp(2) > 0.0f ? thrust_sp(2) : 0.0f;
/* convert back to local frame (NED) */
thrust_sp = _R * thrust_sp_body;
/* set velocity setpoint to zero and reset position */
_vel_sp(0) = 0.0f;
_vel_sp(1) = 0.0f;
_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
}