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

mc_pos_control: add method to constrain velocity setpoint

Browse Source
This commit is contained in:
Martina
2018-07-12 16:36:11 -04:00
committed by Daniel Agar
parent 14444af38c
commit f511d3a399
1 changed files with 19 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+19 -7
View File
@@ -381,6 +381,8 @@ private:
float get_vel_close(const matrix::Vector2f &unit_prev_to_current, const matrix::Vector2f &unit_current_to_next);
void constrain_velocity_setpoint();
void set_manual_acceleration_xy(matrix::Vector2f &stick_input_xy_NED);
void set_manual_acceleration_z(float &max_acc_z, const float stick_input_z_NED);
@@ -965,6 +967,22 @@ MulticopterPositionControl::get_vel_close(const matrix::Vector2f &unit_prev_to_c
}
void
MulticopterPositionControl::constrain_velocity_setpoint()
{
/* make sure velocity setpoint is constrained in all directions (xyz) */
float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1));
if (vel_norm_xy > _vel_max_xy) {
_vel_sp(0) = _vel_sp(0) * _vel_max_xy / vel_norm_xy;
_vel_sp(1) = _vel_sp(1) * _vel_max_xy / vel_norm_xy;
}
_vel_sp(2) = math::constrain(_vel_sp(2), -_vel_max_up.get(), _vel_max_down.get());
}
float
MulticopterPositionControl::get_cruising_speed_xy()
{
@@ -2461,19 +2479,13 @@ MulticopterPositionControl::calculate_velocity_setpoint()
set_takeoff_velocity(_vel_sp(2));
}
constrain_velocity_setpoint();
/* make sure velocity setpoint is constrained in all directions (xyz) */
float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1));
/* check if the velocity demand is significant */
_vel_sp_significant = vel_norm_xy > 0.5f * _vel_max_xy;
if (vel_norm_xy > _vel_max_xy) {
_vel_sp(0) = _vel_sp(0) * _vel_max_xy / vel_norm_xy;
_vel_sp(1) = _vel_sp(1) * _vel_max_xy / vel_norm_xy;
}
_vel_sp(2) = math::constrain(_vel_sp(2), -_vel_max_up.get(), _vel_max_down.get());
_vel_sp_prev = _vel_sp;
}