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

Modified velocity saturation to maintain direction.

Browse Source
This commit is contained in:
James Goppert
2015-06-12 10:58:33 +02:00
committed by Lorenz Meier
parent adbccfaa1c
commit dedd16e36e
1 changed files with 14 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+14 -6
View File
@@ -1032,13 +1032,21 @@ MulticopterPositionControl::task_main()
/* run position & altitude controllers, calculate velocity setpoint */ /* run position & altitude controllers, calculate velocity setpoint */
math::Vector<3> pos_err = _pos_sp - _pos; math::Vector<3> pos_err = _pos_sp - _pos;
/* make sure velocity setpoint is saturated */
_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
for (int i=0; i<3; i++) {
if (_vel_sp(i) > _params.vel_max(i)) { /* make sure velocity setpoint is saturated in xy*/
_vel_sp(i) = _params.vel_max(i); float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) +
} else if (_vel_sp(i) < -_params.vel_max(i)) _vel_sp(1)*_vel_sp(1));
_vel_sp(i) = -_params.vel_max(i); if (vel_norm_xy > _params.vel_max(0)) {
/* note assumes vel_max(0) == vel_max(1) */
_vel_sp(0) = _vel_sp(0)*_params.vel_max(0)/vel_norm_xy;
_vel_sp(1) = _vel_sp(1)*_params.vel_max(1)/vel_norm_xy;
}
/* make sure velocity setpoint is saturated in z*/
float vel_norm_z = sqrtf(_vel_sp(2)*_vel_sp(2));
if (vel_norm_z > _params.vel_max(2)) {
_vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z;
} }
if (!_control_mode.flag_control_altitude_enabled) { if (!_control_mode.flag_control_altitude_enabled) {