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: changed thrust calculation to 3D projection onto body z axis

Browse Source 
until now the desired thrust was simply the absolute value of the thrust setpoint vector
but a multicopter is only capable of producing thrust in its (negative) body z axis
this leads to excess thrust during attitude tracking errors

one important use case is:
trying to land with minimal thrust in NED z axis against gravity
but the position controller still correcting some small horizontal estimation errors
then this prevents unwanted thrust in a completely wrong direction
This commit is contained in:
Matthias Grob 2016-12-29 17:43:10 +01:00 committed by Lorenz Meier
parent d63db203ba
commit 5f3cbbbbc2
1 changed files with 14 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1790,7 +1790,6 @@ MulticopterPositionControl::control_position(float dt)
thr_min = 0.0f;
}
float thrust_abs = thrust_sp.length();
float tilt_max = _params.tilt_max_air;
float thr_max = _params.thr_max;
/* filter vel_z over 1/8sec */
@ -1906,10 +1905,15 @@ MulticopterPositionControl::control_position(float dt)
thrust_sp(2) *= att_comp;
}
/* limit max thrust */
thrust_abs = thrust_sp.length(); /* recalculate because it might have changed */
/* Calculate desired total thrust amount in body z direction
* Project the desired thrust force vector F onto the vehicles thrust axis -R_z
* to compensate for excess thrust during attitude tracking errors */
matrix::Vector3f R_z(_R(0, 2), _R(1, 2), _R(2, 2));
matrix::Vector3f F(thrust_sp.data);
float thrust_body_z = F.dot(-R_z); /* recalculate because it might have changed */
if (thrust_abs > thr_max) {
/* limit max thrust */
if (thrust_body_z > thr_max) {
if (thrust_sp(2) < 0.0f) {
if (-thrust_sp(2) > thr_max) {
/* thrust Z component is too large, limit it */
@ -1933,15 +1937,17 @@ MulticopterPositionControl::control_position(float dt)
} else {
/* Z component is negative, going down, simply limit thrust vector */
float k = thr_max / thrust_abs;
float k = thr_max / thrust_body_z;
thrust_sp *= k;
saturation_xy = true;
saturation_z = true;
}
thrust_abs = thr_max;
thrust_body_z = thr_max;
}
_att_sp.thrust = thrust_body_z;
/* update integrals */
if (_control_mode.flag_control_velocity_enabled && !saturation_xy) {
_thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
@ -1959,8 +1965,8 @@ MulticopterPositionControl::control_position(float dt)
math::Vector<3> body_y;
math::Vector<3> body_z;
if (thrust_abs > SIGMA) {
body_z = -thrust_sp / thrust_abs;
if (thrust_sp.length() > SIGMA) {
body_z = -thrust_sp.normalized();
} else {
/* no thrust, set Z axis to safe value */
@ -2024,8 +2030,6 @@ MulticopterPositionControl::control_position(float dt)
_att_sp.pitch_body = 0.0f;
}
_att_sp.thrust = thrust_abs;
/* save thrust setpoint for logging */
_local_pos_sp.acc_x = thrust_sp(0) * ONE_G;
_local_pos_sp.acc_y = thrust_sp(1) * ONE_G;