dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #2485 from UAVenture/mp_mpc_vel_max 

Add max velocity constraints to multiplatform MPC
This commit is contained in:
Lorenz Meier 2015-06-28 13:41:29 +02:00
parent 60b8c28be2 b02c4ec356
commit a0b89e55a9
1 changed files with 15 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
View File

@ -647,6 +647,21 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
/* make sure velocity setpoint is saturated in xy*/
float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) +
_vel_sp(1)*_vel_sp(1));
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->data().flag_control_altitude_enabled) { if (!_control_mode->data().flag_control_altitude_enabled) {
_reset_alt_sp = true; _reset_alt_sp = true;
_vel_sp(2) = 0.0f; _vel_sp(2) = 0.0f;