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

Acceleration limits: Limit at correct location

Browse Source
This commit is contained in:
Lorenz Meier
2015-12-18 15:55:16 +00:00
parent 4f69834761
commit 7b06c4afb9
1 changed files with 16 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+16 -17
View File
@@ -1201,23 +1201,6 @@ MulticopterPositionControl::task_main()
control_auto(dt); control_auto(dt);
} }
// limit total horizontal acceleration
math::Vector<2> acc_hor;
acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt;
acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt;
if (acc_hor.length() > _params.acc_hor_max) {
acc_hor.normalize();
acc_hor *= _params.acc_hor_max;
math::Vector<2> vel_sp_hor_prev(_vel_sp_prev(0), _vel_sp_prev(1));
math::Vector<2> vel_sp_hor = acc_hor * dt + vel_sp_hor_prev;
_vel_sp(0) = vel_sp_hor(0);
_vel_sp(1) = vel_sp_hor(1);
}
_vel_sp_prev = _vel_sp;
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */ /* idle state, don't run controller and set zero thrust */
@@ -1291,6 +1274,22 @@ MulticopterPositionControl::task_main()
_vel_sp(2) = _params.land_speed; _vel_sp(2) = _params.land_speed;
} }
// limit total horizontal acceleration
math::Vector<2> acc_hor;
acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt;
acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt;
if (acc_hor.length() > _params.acc_hor_max) {
acc_hor.normalize();
acc_hor *= _params.acc_hor_max;
math::Vector<2> vel_sp_hor_prev(_vel_sp_prev(0), _vel_sp_prev(1));
math::Vector<2> vel_sp_hor = acc_hor * dt + vel_sp_hor_prev;
_vel_sp(0) = vel_sp_hor(0);
_vel_sp(1) = vel_sp_hor(1);
}
_vel_sp_prev = _vel_sp;
_global_vel_sp.vx = _vel_sp(0); _global_vel_sp.vx = _vel_sp(0);
_global_vel_sp.vy = _vel_sp(1); _global_vel_sp.vy = _vel_sp(1);
_global_vel_sp.vz = _vel_sp(2); _global_vel_sp.vz = _vel_sp(2);