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

mc_pos_control: fixed float literals and commented calculations

Browse Source
This commit is contained in:
Matthias Grob
2017-01-16 08:25:26 -08:00
committed by Lorenz Meier
parent f95fb0f20f
commit 39f9647291
1 changed files with 5 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+5 -4
View File
@@ -1759,7 +1759,7 @@ MulticopterPositionControl::control_position(float dt)
} else {
thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d)
+ _thrust_int - math::Vector<3>(0, 0, _params.thr_hover);
+ _thrust_int - math::Vector<3>(0.0f, 0.0f, _params.thr_hover);
}
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
@@ -1905,9 +1905,10 @@ MulticopterPositionControl::control_position(float dt)
thrust_sp(2) *= att_comp;
}
/* 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 */
/* Calculate desired total thrust amount in body z direction. */
/* To compensate for excess thrust during attitude tracking errors we
* project the desired thrust force vector F onto the real vehicle's thrust axis in NED:
* body thrust axis [0,0,-1]' rotated by R is: R*[0,0,-1]' = -R_z */
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 */